diff --git a/src/Snake/snake.cpp b/src/Snake/snake.cpp index 67c524a624dfab5591c72eede9aa13bd6ea149f1..352b206abe6db55aacd7f15760c676883bbdb823 100644 --- a/src/Snake/snake.cpp +++ b/src/Snake/snake.cpp @@ -2,13 +2,13 @@ #include "snake.h" -#include #include +#include #include -#include -#include #include +#include +#include #include "clipper/clipper.hpp" #define CLIPPER_SCALE 10000 @@ -34,467 +34,434 @@ namespace snake { // Geometry stuff. //========================================================================= -void polygonCenter(const BoostPolygon &polygon, BoostPoint ¢er) -{ - using namespace mapbox; -if (polygon.outer().empty()) +void polygonCenter(const BoostPolygon &polygon, BoostPoint ¢er) { + using namespace mapbox; + if (polygon.outer().empty()) return; -geometry::polygon p; -geometry::linear_ring lr1; -for (size_t i = 0; i < polygon.outer().size(); ++i) { - geometry::point vertex(polygon.outer()[i].get<0>(), polygon.outer()[i].get<1>()); + geometry::polygon p; + geometry::linear_ring lr1; + for (size_t i = 0; i < polygon.outer().size(); ++i) { + geometry::point vertex(polygon.outer()[i].get<0>(), + polygon.outer()[i].get<1>()); lr1.push_back(vertex); -} -p.push_back(lr1); -geometry::point c = polylabel(p); + } + p.push_back(lr1); + geometry::point c = polylabel(p); -center.set<0>(c.x); -center.set<1>(c.y); + center.set<0>(c.x); + center.set<1>(c.y); } -void minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox) -{ - /* - Find the minimum-area bounding box of a set of 2D points - - The input is a 2D convex hull, in an Nx2 numpy array of x-y co-ordinates. - The first and last points points must be the same, making a closed polygon. - This program finds the rotation angles of each edge of the convex polygon, - then tests the area of a bounding box aligned with the unique angles in - 90 degrees of the 1st Quadrant. - Returns the - - Tested with Python 2.6.5 on Ubuntu 10.04.4 (original version) - Results verified using Matlab - - Copyright (c) 2013, David Butterworth, University of Queensland - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of the Willow Garage, Inc. nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - POSSIBILITY OF SUCH DAMAGE. - */ - - if (polygon.outer().empty()) - return; - BoostPolygon convex_hull; - bg::convex_hull(polygon, convex_hull); - - //cout << "Convex hull: " << bg::wkt(convex_hull) << endl; - - //# Compute edges (x2-x1,y2-y1) - std::vector edges; - auto convex_hull_outer = convex_hull.outer(); - for (long i=0; i < long(convex_hull_outer.size())-1; ++i) { - BoostPoint p1 = convex_hull_outer.at(i); - BoostPoint p2 = convex_hull_outer.at(i+1); - double edge_x = p2.get<0>() - p1.get<0>(); - double edge_y = p2.get<1>() - p1.get<1>(); - edges.push_back(BoostPoint{edge_x, edge_y}); - } - -// cout << "Edges: "; -// for (auto e : edges) -// cout << e.get<0>() << " " << e.get<1>() << ","; -// cout << endl; - - // Calculate unique edge angles atan2(y/x) - double angle_scale = 1e3; - std::set angles_long; - for (auto vertex : edges) { - double angle = std::fmod(atan2(vertex.get<1>(), vertex.get<0>()), M_PI / 2); - angle = angle < 0 ? angle + M_PI / 2 : angle; // want strictly positive answers - angles_long.insert(long(round(angle*angle_scale))); - } - std::vector edge_angles; - for (auto a : angles_long) - edge_angles.push_back(double(a)/angle_scale); - - -// cout << "Unique angles: "; -// for (auto e : edge_angles) -// cout << e*180/M_PI << ","; -// cout << endl; - - double min_area = std::numeric_limits::infinity(); - // Test each angle to find bounding box with smallest area - // print "Testing", len(edge_angles), "possible rotations for bounding box... \n" - for (double angle : edge_angles){ - - trans::rotate_transformer rotate(angle*180/M_PI); - BoostPolygon hull_rotated; - bg::transform(convex_hull, hull_rotated, rotate); - //cout << "Convex hull rotated: " << bg::wkt(hull_rotated) << endl; - - bg::model::box box; - bg::envelope(hull_rotated, box); -// cout << "Bounding box: " << bg::wkt>(box) << endl; - - //# print "Rotated hull points are \n", rot_points - BoostPoint min_corner = box.min_corner(); - BoostPoint max_corner = box.max_corner(); - double min_x = min_corner.get<0>(); - double max_x = max_corner.get<0>(); - double min_y = min_corner.get<1>(); - double max_y = max_corner.get<1>(); -// cout << "min_x: " << min_x << endl; -// cout << "max_x: " << max_x << endl; -// cout << "min_y: " << min_y << endl; -// cout << "max_y: " << max_y << endl; - - // Calculate height/width/area of this bounding rectangle - double width = max_x - min_x; - double height = max_y - min_y; - double area = width * height; -// cout << "Width: " << width << endl; -// cout << "Height: " << height << endl; -// cout << "area: " << area << endl; -// cout << "angle: " << angle*180/M_PI << endl; - - // Store the smallest rect found first (a simple convex hull might have 2 answers with same area) - if (area < min_area){ - min_area = area; - minBBox.angle = angle; - minBBox.width = width; - minBBox.height = height; - - minBBox.corners.clear(); - minBBox.corners.outer().push_back(BoostPoint{min_x, min_y}); - minBBox.corners.outer().push_back(BoostPoint{min_x, max_y}); - minBBox.corners.outer().push_back(BoostPoint{max_x, max_y}); - minBBox.corners.outer().push_back(BoostPoint{max_x, min_y}); - minBBox.corners.outer().push_back(BoostPoint{min_x, min_y}); - } - //cout << endl << endl; - } - - - // Transform corners of minimal bounding box. - trans::rotate_transformer rotate(-minBBox.angle*180/M_PI); - BoostPolygon rotated_polygon; - bg::transform(minBBox.corners, rotated_polygon, rotate); - minBBox.corners = rotated_polygon; +void minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox) { + /* + Find the minimum-area bounding box of a set of 2D points + + The input is a 2D convex hull, in an Nx2 numpy array of x-y co-ordinates. + The first and last points points must be the same, making a closed polygon. + This program finds the rotation angles of each edge of the convex polygon, + then tests the area of a bounding box aligned with the unique angles in + 90 degrees of the 1st Quadrant. + Returns the + + Tested with Python 2.6.5 on Ubuntu 10.04.4 (original version) + Results verified using Matlab + + Copyright (c) 2013, David Butterworth, University of Queensland + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Willow Garage, Inc. nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. + */ + + if (polygon.outer().empty()) + return; + BoostPolygon convex_hull; + bg::convex_hull(polygon, convex_hull); + + // cout << "Convex hull: " << bg::wkt(convex_hull) << endl; + + //# Compute edges (x2-x1,y2-y1) + std::vector edges; + auto convex_hull_outer = convex_hull.outer(); + for (long i = 0; i < long(convex_hull_outer.size()) - 1; ++i) { + BoostPoint p1 = convex_hull_outer.at(i); + BoostPoint p2 = convex_hull_outer.at(i + 1); + double edge_x = p2.get<0>() - p1.get<0>(); + double edge_y = p2.get<1>() - p1.get<1>(); + edges.push_back(BoostPoint{edge_x, edge_y}); + } + + // cout << "Edges: "; + // for (auto e : edges) + // cout << e.get<0>() << " " << e.get<1>() << ","; + // cout << endl; + + // Calculate unique edge angles atan2(y/x) + double angle_scale = 1e3; + std::set angles_long; + for (auto vertex : edges) { + double angle = std::fmod(atan2(vertex.get<1>(), vertex.get<0>()), M_PI / 2); + angle = + angle < 0 ? angle + M_PI / 2 : angle; // want strictly positive answers + angles_long.insert(long(round(angle * angle_scale))); + } + std::vector edge_angles; + for (auto a : angles_long) + edge_angles.push_back(double(a) / angle_scale); + + // cout << "Unique angles: "; + // for (auto e : edge_angles) + // cout << e*180/M_PI << ","; + // cout << endl; + + double min_area = std::numeric_limits::infinity(); + // Test each angle to find bounding box with smallest area + // print "Testing", len(edge_angles), "possible rotations for bounding box... + // \n" + for (double angle : edge_angles) { + + trans::rotate_transformer rotate(angle * 180 / + M_PI); + BoostPolygon hull_rotated; + bg::transform(convex_hull, hull_rotated, rotate); + // cout << "Convex hull rotated: " << bg::wkt(hull_rotated) + // << endl; + + bg::model::box box; + bg::envelope(hull_rotated, box); + // cout << "Bounding box: " << + // bg::wkt>(box) << endl; + + //# print "Rotated hull points are \n", rot_points + BoostPoint min_corner = box.min_corner(); + BoostPoint max_corner = box.max_corner(); + double min_x = min_corner.get<0>(); + double max_x = max_corner.get<0>(); + double min_y = min_corner.get<1>(); + double max_y = max_corner.get<1>(); + // cout << "min_x: " << min_x << endl; + // cout << "max_x: " << max_x << endl; + // cout << "min_y: " << min_y << endl; + // cout << "max_y: " << max_y << endl; + + // Calculate height/width/area of this bounding rectangle + double width = max_x - min_x; + double height = max_y - min_y; + double area = width * height; + // cout << "Width: " << width << endl; + // cout << "Height: " << height << endl; + // cout << "area: " << area << endl; + // cout << "angle: " << angle*180/M_PI << endl; + + // Store the smallest rect found first (a simple convex hull might have 2 + // answers with same area) + if (area < min_area) { + min_area = area; + minBBox.angle = angle; + minBBox.width = width; + minBBox.height = height; + + minBBox.corners.clear(); + minBBox.corners.outer().push_back(BoostPoint{min_x, min_y}); + minBBox.corners.outer().push_back(BoostPoint{min_x, max_y}); + minBBox.corners.outer().push_back(BoostPoint{max_x, max_y}); + minBBox.corners.outer().push_back(BoostPoint{max_x, min_y}); + minBBox.corners.outer().push_back(BoostPoint{min_x, min_y}); + } + // cout << endl << endl; + } + + // Transform corners of minimal bounding box. + trans::rotate_transformer rotate(-minBBox.angle * + 180 / M_PI); + BoostPolygon rotated_polygon; + bg::transform(minBBox.corners, rotated_polygon, rotate); + minBBox.corners = rotated_polygon; } -void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset, double offset) -{ - bg::strategy::buffer::distance_symmetric distance_strategy(offset); - bg::strategy::buffer::join_miter join_strategy(3); - bg::strategy::buffer::end_flat end_strategy; - bg::strategy::buffer::point_square point_strategy; - bg::strategy::buffer::side_straight side_strategy; +void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset, + double offset) { + bg::strategy::buffer::distance_symmetric distance_strategy(offset); + bg::strategy::buffer::join_miter join_strategy(3); + bg::strategy::buffer::end_flat end_strategy; + bg::strategy::buffer::point_square point_strategy; + bg::strategy::buffer::side_straight side_strategy; + bg::model::multi_polygon result; - bg::model::multi_polygon result; - - bg::buffer(polygon, result, distance_strategy, side_strategy, join_strategy, end_strategy, point_strategy); - - if (result.size() > 0) - polygonOffset = result[0]; + bg::buffer(polygon, result, distance_strategy, side_strategy, join_strategy, + end_strategy, point_strategy); + if (result.size() > 0) + polygonOffset = result[0]; } +void graphFromPolygon(const BoostPolygon &polygon, + const BoostLineString &vertices, Matrix &graph) { + size_t n = graph.getN(); -void graphFromPolygon(const BoostPolygon &polygon, const BoostLineString &vertices, Matrix &graph) -{ - size_t n = graph.getN(); - - for (size_t i=0; i < n; ++i) { - BoostPoint v1 = vertices[i]; - for (size_t j=i+1; j < n; ++j){ - BoostPoint v2 = vertices[j]; - BoostLineString path{v1, v2}; + for (size_t i = 0; i < n; ++i) { + BoostPoint v1 = vertices[i]; + for (size_t j = i + 1; j < n; ++j) { + BoostPoint v2 = vertices[j]; + BoostLineString path{v1, v2}; - double distance = 0; - if (!bg::within(path, polygon)) - distance = std::numeric_limits::infinity(); - else - distance = bg::length(path); - - graph.set(i, j, distance); - graph.set(j, i, distance); - } - } + double distance = 0; + if (!bg::within(path, polygon)) + distance = std::numeric_limits::infinity(); + else + distance = bg::length(path); + graph.set(i, j, distance); + graph.set(j, i, distance); + } + } } -bool dijkstraAlgorithm(const size_t numElements, - size_t startIndex, - size_t endIndex, - std::vector &elementPath, - std::function distanceDij) -{ - if ( startIndex >= numElements - || endIndex >= numElements - || endIndex == startIndex) { - return false; - } - // Node struct - // predecessorIndex is the index of the predecessor node (nodeList[predecessorIndex]) - // distance is the distance between the node and the start node - // node number is stored by the position in nodeList - struct Node{ - int predecessorIndex = -1; - double distance = std::numeric_limits::infinity(); - }; - - // The list with all Nodes (elements) - std::vector nodeList(numElements); - // This list will be initalized with indices referring to the elements of nodeList. - // Elements will be successively remove during the execution of the Dijkstra Algorithm. - std::vector workingSet(numElements); - - //append elements to node list - for (size_t i = 0; i < numElements; ++i) workingSet[i] = i; - - - nodeList[startIndex].distance = 0; - - // Dijkstra Algorithm - // https://de.wikipedia.org/wiki/Dijkstra-Algorithmus - while (workingSet.size() > 0) { - // serach Node with minimal distance - double minDist = std::numeric_limits::infinity(); - int minDistIndex_WS = -1; // WS = workinSet - for (size_t i = 0; i < workingSet.size(); ++i) { - const int nodeIndex = workingSet.at(i); - const double dist = nodeList.at(nodeIndex).distance; - if (dist < minDist) { - minDist = dist; - minDistIndex_WS = i; - } - } - if (minDistIndex_WS == -1) - return false; - - size_t indexU_NL = workingSet.at(minDistIndex_WS); // NL = nodeList - workingSet.erase(workingSet.begin()+minDistIndex_WS); - if (indexU_NL == endIndex) // shortest path found - break; - - const double distanceU = nodeList.at(indexU_NL).distance; - //update distance - for (size_t i = 0; i < workingSet.size(); ++i) { - int indexV_NL = workingSet[i]; // NL = nodeList - Node* v = &nodeList[indexV_NL]; - double dist = distanceDij(indexU_NL, indexV_NL); - // is ther an alternative path which is shorter? - double alternative = distanceU + dist; - if (alternative < v->distance) { - v->distance = alternative; - v->predecessorIndex = indexU_NL; - } - } - - } - // end Djikstra Algorithm - - - // reverse assemble path - int e = endIndex; - while (1) { - if (e == -1) { - if (elementPath[0] == startIndex)// check if starting point was reached - break; - return false; - } - elementPath.insert(elementPath.begin(), e); - - //Update Node - e = nodeList[e].predecessorIndex; - - } - return true; -} +bool dijkstraAlgorithm( + const size_t numElements, size_t startIndex, size_t endIndex, + std::vector &elementPath, + std::function distanceDij) { + if (startIndex >= numElements || endIndex >= numElements || + endIndex == startIndex) { + return false; + } + // Node struct + // predecessorIndex is the index of the predecessor node + // (nodeList[predecessorIndex]) distance is the distance between the node and + // the start node node number is stored by the position in nodeList + struct Node { + int predecessorIndex = -1; + double distance = std::numeric_limits::infinity(); + }; + + // The list with all Nodes (elements) + std::vector nodeList(numElements); + // This list will be initalized with indices referring to the elements of + // nodeList. Elements will be successively remove during the execution of the + // Dijkstra Algorithm. + std::vector workingSet(numElements); + + // append elements to node list + for (size_t i = 0; i < numElements; ++i) + workingSet[i] = i; + + nodeList[startIndex].distance = 0; + + // Dijkstra Algorithm + // https://de.wikipedia.org/wiki/Dijkstra-Algorithmus + while (workingSet.size() > 0) { + // serach Node with minimal distance + double minDist = std::numeric_limits::infinity(); + int minDistIndex_WS = -1; // WS = workinSet + for (size_t i = 0; i < workingSet.size(); ++i) { + const int nodeIndex = workingSet.at(i); + const double dist = nodeList.at(nodeIndex).distance; + if (dist < minDist) { + minDist = dist; + minDistIndex_WS = i; + } + } + if (minDistIndex_WS == -1) + return false; + + size_t indexU_NL = workingSet.at(minDistIndex_WS); // NL = nodeList + workingSet.erase(workingSet.begin() + minDistIndex_WS); + if (indexU_NL == endIndex) // shortest path found + break; + + const double distanceU = nodeList.at(indexU_NL).distance; + // update distance + for (size_t i = 0; i < workingSet.size(); ++i) { + int indexV_NL = workingSet[i]; // NL = nodeList + Node *v = &nodeList[indexV_NL]; + double dist = distanceDij(indexU_NL, indexV_NL); + // is ther an alternative path which is shorter? + double alternative = distanceU + dist; + if (alternative < v->distance) { + v->distance = alternative; + v->predecessorIndex = indexU_NL; + } + } + } + // end Djikstra Algorithm + + // reverse assemble path + int e = endIndex; + while (1) { + if (e == -1) { + if (elementPath[0] == startIndex) // check if starting point was reached + break; + return false; + } + elementPath.insert(elementPath.begin(), e); -void toDistanceMatrix(Matrix &graph) -{ - size_t n = graph.getN(); - - auto distance = [graph](size_t i, size_t j){ - return graph.get(i,j); - }; - - - std::vector path; - for (size_t i=0; i < n; ++i) { - for (size_t j=i+1; j < n; ++j){ - double d = graph.get(i,j); - if (!std::isinf(d)) - continue; - path.clear(); - bool ret = dijkstraAlgorithm(n, i, j, path, distance); - assert(ret); - (void)ret; -// cout << "(" << i << "," << j << ") d: " << d << endl; -// cout << "Path size: " << path.size() << endl; -// for (auto idx : path) -// cout << idx << " "; -// cout << endl; - - d = 0; - for (long k=0; k < long(path.size())-1; ++k) { - size_t idx0 = path[k]; - size_t idx1 = path[k+1]; - double d0 = graph.get(idx0, idx1); - assert(std::isinf(d0) == false); - d += d0; - } - - graph.set(i, j, d); - graph.set(j, i, d); - } - } + // Update Node + e = nodeList[e].predecessorIndex; + } + return true; } -void shortestPathFromGraph(const Matrix &graph, size_t startIndex, size_t endIndex, std::vector &pathIdx) -{ - - if (!std::isinf(graph.get(startIndex, endIndex))){ - pathIdx.push_back(startIndex); - pathIdx.push_back(endIndex); - } else { - auto distance = [graph](size_t i, size_t j){ - return graph.get(i, j); - }; - bool ret = dijkstraAlgorithm(graph.getN(), startIndex, endIndex, pathIdx, distance); - assert(ret); - (void)ret; - } - -//========================================================================= -// Scenario calculation. -//========================================================================= -}Scenario::Scenario() : - _tileWidth(5*bu::si::meter) - , _tileHeight(5*bu::si::meter) - , _minTileArea(0*bu::si::meter*bu::si::meter) - , _needsUpdate(true) -{ +void toDistanceMatrix(Matrix &graph) { + size_t n = graph.getN(); + + auto distance = [graph](size_t i, size_t j) { return graph.get(i, j); }; + + std::vector path; + for (size_t i = 0; i < n; ++i) { + for (size_t j = i + 1; j < n; ++j) { + double d = graph.get(i, j); + if (!std::isinf(d)) + continue; + path.clear(); + bool ret = dijkstraAlgorithm(n, i, j, path, distance); + assert(ret); + (void)ret; + // cout << "(" << i << "," << j << ") d: " << d << endl; + // cout << "Path size: " << path.size() << endl; + // for (auto idx : path) + // cout << idx << " "; + // cout << endl; + + d = 0; + for (long k = 0; k < long(path.size()) - 1; ++k) { + size_t idx0 = path[k]; + size_t idx1 = path[k + 1]; + double d0 = graph.get(idx0, idx1); + assert(std::isinf(d0) == false); + d += d0; + } + + graph.set(i, j, d); + graph.set(j, i, d); + } + } +} +void shortestPathFromGraph(const Matrix &graph, size_t startIndex, + size_t endIndex, std::vector &pathIdx) { + + if (!std::isinf(graph.get(startIndex, endIndex))) { + pathIdx.push_back(startIndex); + pathIdx.push_back(endIndex); + } else { + auto distance = [graph](size_t i, size_t j) { return graph.get(i, j); }; + bool ret = dijkstraAlgorithm(graph.getN(), startIndex, endIndex, pathIdx, + distance); + assert(ret); + (void)ret; + } + + //========================================================================= + // Scenario calculation. + //========================================================================= } +Scenario::Scenario() + : _tileWidth(5 * bu::si::meter), _tileHeight(5 * bu::si::meter), + _minTileArea(0 * bu::si::meter * bu::si::meter), _needsUpdate(true) {} -void Scenario::setMeasurementArea(const BoostPolygon &area) -{ - _needsUpdate = true; - _mArea = area; +void Scenario::setMeasurementArea(const BoostPolygon &area) { + _needsUpdate = true; + _mArea = area; } -void Scenario::setServiceArea(const BoostPolygon &area) -{ - _needsUpdate = true; - _sArea = area; +void Scenario::setServiceArea(const BoostPolygon &area) { + _needsUpdate = true; + _sArea = area; } -void Scenario::setCorridor(const BoostPolygon &area) -{ - _needsUpdate = true; - _corridor = area; +void Scenario::setCorridor(const BoostPolygon &area) { + _needsUpdate = true; + _corridor = area; } BoostPolygon &Scenario::measurementArea() { - _needsUpdate = true; - return _mArea; + _needsUpdate = true; + return _mArea; } BoostPolygon &Scenario::serviceArea() { - _needsUpdate = true; - return _sArea; + _needsUpdate = true; + return _sArea; } BoostPolygon &Scenario::corridor() { - _needsUpdate = true; - return _corridor; + _needsUpdate = true; + return _corridor; } -const BoundingBox &Scenario::mAreaBoundingBox() const -{ - return _mAreaBoundingBox; +const BoundingBox &Scenario::mAreaBoundingBox() const { + return _mAreaBoundingBox; } -const BoostPolygon &Scenario::measurementArea() const -{ - return _mArea; -} +const BoostPolygon &Scenario::measurementArea() const { return _mArea; } -const BoostPolygon &Scenario::serviceArea() const -{ - return _sArea; -} +const BoostPolygon &Scenario::serviceArea() const { return _sArea; } -const BoostPolygon &Scenario::corridor() const -{ - return _corridor; -} +const BoostPolygon &Scenario::corridor() const { return _corridor; } -const BoostPolygon &Scenario::joinedArea() const { - return _jArea; -} +const BoostPolygon &Scenario::joinedArea() const { return _jArea; } -const vector &Scenario::tiles() const{ - return _tiles; -} +const vector &Scenario::tiles() const { return _tiles; } -const BoostLineString &Scenario::tileCenterPoints() const{ - return _tileCenterPoints; +const BoostLineString &Scenario::tileCenterPoints() const { + return _tileCenterPoints; } -const BoundingBox &Scenario::measurementAreaBBox() const{ - return _mAreaBoundingBox; +const BoundingBox &Scenario::measurementAreaBBox() const { + return _mAreaBoundingBox; } -const BoostPoint &Scenario::homePositon() const{ - return _homePosition; -} +const BoostPoint &Scenario::homePositon() const { return _homePosition; } -bool Scenario::update() -{ - if ( !_needsUpdate ) - return true; - if (!_calculateBoundingBox()) - return false; - if (!_calculateTiles()) - return false; - if (!_calculateJoinedArea()) - return false; - _needsUpdate = false; +bool Scenario::update() { + if (!_needsUpdate) return true; + if (!_calculateBoundingBox()) + return false; + if (!_calculateTiles()) + return false; + if (!_calculateJoinedArea()) + return false; + _needsUpdate = false; + return true; } -bool Scenario::_calculateBoundingBox() -{ - minimalBoundingBox(_mArea, _mAreaBoundingBox); - return true; +bool Scenario::_calculateBoundingBox() { + minimalBoundingBox(_mArea, _mAreaBoundingBox); + return true; } - /** - * Devides the (measurement area) bounding box into tiles and clips it to the measurement area. + * Devides the (measurement area) bounding box into tiles and clips it to the + * measurement area. * - * Devides the (measurement area) bounding box into tiles of width \p tileWidth and height \p tileHeight. - * Clips the resulting tiles to the measurement area. Tiles are rejected, if their area is smaller than \p minTileArea. - * The function assumes that \a _mArea and \a _mAreaBoundingBox have correct values. \see \ref Scenario::_areas2enu() and \ref - * Scenario::_calculateBoundingBox(). + * Devides the (measurement area) bounding box into tiles of width \p tileWidth + * and height \p tileHeight. Clips the resulting tiles to the measurement area. + * Tiles are rejected, if their area is smaller than \p minTileArea. The + * function assumes that \a _mArea and \a _mAreaBoundingBox have correct values. + * \see \ref Scenario::_areas2enu() and \ref Scenario::_calculateBoundingBox(). * * @param tileWidth The width (>0) of a tile. * @param tileHeight The heigth (>0) of a tile. @@ -502,585 +469,585 @@ bool Scenario::_calculateBoundingBox() * * @return Returns true if successful. */ -bool Scenario::_calculateTiles() -{ - _tiles.clear(); - _tileCenterPoints.clear(); - - if (_tileWidth <= 0*bu::si::meter - || _tileHeight <= 0*bu::si::meter - || _minTileArea <= 0*bu::si::meter*bu::si::meter - ) - { - errorString = "Parameters tileWidth, tileHeight, minTileArea must be positive."; - return false; - } - - double bboxWidth = _mAreaBoundingBox.width; - double bboxHeight = _mAreaBoundingBox.height; - - BoostPoint origin = _mAreaBoundingBox.corners.outer()[0]; - - //cout << "Origin: " << origin[0] << " " << origin[1] << endl; - // Transform _mArea polygon to bounding box coordinate system. - trans::rotate_transformer rotate(_mAreaBoundingBox.angle*180/M_PI); - trans::translate_transformer translate(-origin.get<0>(), -origin.get<1>()); - BoostPolygon translated_polygon; - BoostPolygon rotated_polygon; - boost::geometry::transform(_mArea, translated_polygon, translate); - boost::geometry::transform(translated_polygon, rotated_polygon, rotate); - bg::correct(rotated_polygon); - - //cout << bg::wkt(rotated_polygon) << endl; - - size_t iMax = ceil(bboxWidth/_tileWidth.value()); - size_t jMax = ceil(bboxHeight/_tileHeight.value()); - - if (iMax < 1 || jMax < 1) { - std::stringstream ss; - ss << "Tile width or Tile height to large. " - << "tile width = " << _tileWidth << ", " - << "tile height = " << _tileHeight << "." << std::endl; - errorString = ss.str(); - return false; - } - - - trans::rotate_transformer rotate_back(-_mAreaBoundingBox.angle*180/M_PI); - trans::translate_transformer translate_back(origin.get<0>(), origin.get<1>()); - for (size_t i = 0; i < iMax; ++i){ - double x_min = _tileWidth.value()*i; - double x_max = x_min + _tileWidth.value(); - for (size_t j = 0; j < jMax; ++j){ - double y_min = _tileHeight.value()*j; - double y_max = y_min + _tileHeight.value(); - - BoostPolygon tile_unclipped; - tile_unclipped.outer().push_back(BoostPoint{x_min, y_min}); - tile_unclipped.outer().push_back(BoostPoint{x_min, y_max}); - tile_unclipped.outer().push_back(BoostPoint{x_max, y_max}); - tile_unclipped.outer().push_back(BoostPoint{x_max, y_min}); - tile_unclipped.outer().push_back(BoostPoint{x_min, y_min}); - - - std::deque boost_tiles; - if (!boost::geometry::intersection(tile_unclipped, rotated_polygon, boost_tiles)) - continue; - - for (BoostPolygon t : boost_tiles) - { - if (bg::area(t) > _minTileArea.value()){ - // Transform boost_tile to world coordinate system. - BoostPolygon rotated_tile; - BoostPolygon translated_tile; - boost::geometry::transform(t, rotated_tile, rotate_back); - boost::geometry::transform(rotated_tile, translated_tile, translate_back); - - // Store tile and calculate center point. - _tiles.push_back(translated_tile); - BoostPoint tile_center; - polygonCenter(translated_tile, tile_center); - _tileCenterPoints.push_back(tile_center); - } - } +bool Scenario::_calculateTiles() { + _tiles.clear(); + _tileCenterPoints.clear(); + + if (_tileWidth <= 0 * bu::si::meter || _tileHeight <= 0 * bu::si::meter || + _minTileArea <= 0 * bu::si::meter * bu::si::meter) { + errorString = + "Parameters tileWidth, tileHeight, minTileArea must be positive."; + return false; + } + + double bboxWidth = _mAreaBoundingBox.width; + double bboxHeight = _mAreaBoundingBox.height; + + BoostPoint origin = _mAreaBoundingBox.corners.outer()[0]; + + // cout << "Origin: " << origin[0] << " " << origin[1] << endl; + // Transform _mArea polygon to bounding box coordinate system. + trans::rotate_transformer rotate( + _mAreaBoundingBox.angle * 180 / M_PI); + trans::translate_transformer translate(-origin.get<0>(), + -origin.get<1>()); + BoostPolygon translated_polygon; + BoostPolygon rotated_polygon; + boost::geometry::transform(_mArea, translated_polygon, translate); + boost::geometry::transform(translated_polygon, rotated_polygon, rotate); + bg::correct(rotated_polygon); + + // cout << bg::wkt(rotated_polygon) << endl; + + size_t iMax = ceil(bboxWidth / _tileWidth.value()); + size_t jMax = ceil(bboxHeight / _tileHeight.value()); + + if (iMax < 1 || jMax < 1) { + std::stringstream ss; + ss << "Tile width or Tile height to large. " + << "tile width = " << _tileWidth << ", " + << "tile height = " << _tileHeight << "." << std::endl; + errorString = ss.str(); + return false; + } + + trans::rotate_transformer rotate_back( + -_mAreaBoundingBox.angle * 180 / M_PI); + trans::translate_transformer translate_back(origin.get<0>(), + origin.get<1>()); + for (size_t i = 0; i < iMax; ++i) { + double x_min = _tileWidth.value() * i; + double x_max = x_min + _tileWidth.value(); + for (size_t j = 0; j < jMax; ++j) { + double y_min = _tileHeight.value() * j; + double y_max = y_min + _tileHeight.value(); + + BoostPolygon tile_unclipped; + tile_unclipped.outer().push_back(BoostPoint{x_min, y_min}); + tile_unclipped.outer().push_back(BoostPoint{x_min, y_max}); + tile_unclipped.outer().push_back(BoostPoint{x_max, y_max}); + tile_unclipped.outer().push_back(BoostPoint{x_max, y_min}); + tile_unclipped.outer().push_back(BoostPoint{x_min, y_min}); + + std::deque boost_tiles; + if (!boost::geometry::intersection(tile_unclipped, rotated_polygon, + boost_tiles)) + continue; + + for (BoostPolygon t : boost_tiles) { + if (bg::area(t) > _minTileArea.value()) { + // Transform boost_tile to world coordinate system. + BoostPolygon rotated_tile; + BoostPolygon translated_tile; + boost::geometry::transform(t, rotated_tile, rotate_back); + boost::geometry::transform(rotated_tile, translated_tile, + translate_back); + + // Store tile and calculate center point. + _tiles.push_back(translated_tile); + BoostPoint tile_center; + polygonCenter(translated_tile, tile_center); + _tileCenterPoints.push_back(tile_center); } + } } + } - if (_tiles.size() < 1){ - errorString = "No tiles calculated. Is the minTileArea parameter large enough?"; - return false; - } + if (_tiles.size() < 1) { + errorString = + "No tiles calculated. Is the minTileArea parameter large enough?"; + return false; + } - return true; + return true; } -bool Scenario::_calculateJoinedArea() -{ - _jArea.clear(); - // Measurement area and service area overlapping? - bool overlapingSerMeas = bg::intersects(_mArea, _sArea) ? true : false; - bool corridorValid = _corridor.outer().size() > 0 ? true : false; - - - // Check if corridor is connecting measurement area and service area. - bool corridor_is_connection = false; - if (corridorValid) { - // Corridor overlaping with measurement area? - if ( bg::intersects(_corridor, _mArea) ) { - // Corridor overlaping with service area? - if ( bg::intersects(_corridor, _sArea) ) { - corridor_is_connection = true; - } - } +bool Scenario::_calculateJoinedArea() { + _jArea.clear(); + // Measurement area and service area overlapping? + bool overlapingSerMeas = bg::intersects(_mArea, _sArea) ? true : false; + bool corridorValid = _corridor.outer().size() > 0 ? true : false; + + // Check if corridor is connecting measurement area and service area. + bool corridor_is_connection = false; + if (corridorValid) { + // Corridor overlaping with measurement area? + if (bg::intersects(_corridor, _mArea)) { + // Corridor overlaping with service area? + if (bg::intersects(_corridor, _sArea)) { + corridor_is_connection = true; + } } - - // Are areas joinable? - std::deque sol; - BoostPolygon partialArea = _mArea; - if (overlapingSerMeas){ - if(corridor_is_connection){ - bg::union_(partialArea, _corridor, sol); - } - } else if (corridor_is_connection){ - bg::union_(partialArea, _corridor, sol); - } else { - errorString = "Areas are not overlapping"; - return false; - } - - if (sol.size() > 0) { - partialArea = sol[0]; - sol.clear(); + } + + // Are areas joinable? + std::deque sol; + BoostPolygon partialArea = _mArea; + if (overlapingSerMeas) { + if (corridor_is_connection) { + bg::union_(partialArea, _corridor, sol); } - - // Join areas. - bg::union_(partialArea, _sArea, sol); - - if (sol.size() > 0) { - _jArea = sol[0]; - } else { - return false; - } - - return true; + } else if (corridor_is_connection) { + bg::union_(partialArea, _corridor, sol); + } else { + errorString = "Areas are not overlapping"; + return false; + } + + if (sol.size() > 0) { + partialArea = sol[0]; + sol.clear(); + } + + // Join areas. + bg::union_(partialArea, _sArea, sol); + + if (sol.size() > 0) { + _jArea = sol[0]; + } else { + return false; + } + + return true; } -Area Scenario::minTileArea() const -{ - return _minTileArea; -} +Area Scenario::minTileArea() const { return _minTileArea; } -void Scenario::setMinTileArea(Area minTileArea) -{ - if ( minTileArea >= 0*bu::si::meter*bu::si::meter){ - _needsUpdate = true; - _minTileArea = minTileArea; - } +void Scenario::setMinTileArea(Area minTileArea) { + if (minTileArea >= 0 * bu::si::meter * bu::si::meter) { + _needsUpdate = true; + _minTileArea = minTileArea; + } } -Length Scenario::tileHeight() const -{ - return _tileHeight; -} +Length Scenario::tileHeight() const { return _tileHeight; } -void Scenario::setTileHeight(Length tileHeight) -{ - if ( tileHeight > 0*bu::si::meter) { - _needsUpdate = true; - _tileHeight = tileHeight; - } +void Scenario::setTileHeight(Length tileHeight) { + if (tileHeight > 0 * bu::si::meter) { + _needsUpdate = true; + _tileHeight = tileHeight; + } } -Length Scenario::tileWidth() const -{ - return _tileWidth; -} +Length Scenario::tileWidth() const { return _tileWidth; } -void Scenario::setTileWidth(Length tileWidth) -{ - if ( tileWidth > 0*bu::si::meter ){ - _needsUpdate = true; - _tileWidth = tileWidth; - } +void Scenario::setTileWidth(Length tileWidth) { + if (tileWidth > 0 * bu::si::meter) { + _needsUpdate = true; + _tileWidth = tileWidth; + } } //========================================================================= // Tile calculation. //========================================================================= -bool joinAreas(const std::vector &areas, BoostPolygon &joinedArea) -{ - if (areas.size() < 1) - return false; - std::deque idxList; - for(size_t i = 1; i < areas.size(); ++i) - idxList.push_back(i); - joinedArea = areas[0]; - - std::deque sol; - while (idxList.size() > 0){ - bool success = false; - for (auto it = idxList.begin(); it != idxList.end(); ++it){ - bg::union_(joinedArea, areas[*it], sol); - if (sol.size() > 0) { - joinedArea = sol[0]; - sol.clear(); - idxList.erase(it); - success = true; - break; - } - } - if ( !success ) - return false; +bool joinAreas(const std::vector &areas, + BoostPolygon &joinedArea) { + if (areas.size() < 1) + return false; + std::deque idxList; + for (size_t i = 1; i < areas.size(); ++i) + idxList.push_back(i); + joinedArea = areas[0]; + + std::deque sol; + while (idxList.size() > 0) { + bool success = false; + for (auto it = idxList.begin(); it != idxList.end(); ++it) { + bg::union_(joinedArea, areas[*it], sol); + if (sol.size() > 0) { + joinedArea = sol[0]; + sol.clear(); + idxList.erase(it); + success = true; + break; + } } + if (!success) + return false; + } - return true; + return true; } -BoundingBox::BoundingBox() : - width(0) - , height(0) - , angle(0) -{ +BoundingBox::BoundingBox() : width(0), height(0), angle(0) {} +void BoundingBox::clear() { + width = 0; + height = 0; + angle = 0; + corners.clear(); } -void BoundingBox::clear() -{ - width = 0; - height = 0; - angle = 0; - corners.clear(); -} - -bool flight_plan::transectsFromScenario(Length distance, - Length minLength, - Angle angle, - const Scenario &scenario, +bool flight_plan::transectsFromScenario(Length distance, Length minLength, + Angle angle, const Scenario &scenario, const Progress &p, flight_plan::Transects &t, - string &errorString) -{ - // Rotate measurement area by angle and calculate bounding box. - auto &mArea = scenario.measurementArea(); - BoostPolygon mAreaRotated; - trans::rotate_transformer rotate(-angle.value()*180/M_PI); - bg::transform(mArea, mAreaRotated, rotate); - - BoostBox box; - boost::geometry::envelope(mAreaRotated, box); - - double x0 = box.min_corner().get<0>(); - double y0 = box.min_corner().get<1>(); - double x1 = box.max_corner().get<0>(); - double y1 = box.max_corner().get<1>(); - - // Generate transects and convert them to clipper path. - size_t num_t = int(ceil((y1 - y0)/distance.value())); // number of transects - trans::rotate_transformer rotate_back(angle.value()*180/M_PI); - vector transectsClipper; - transectsClipper.reserve(num_t); - for (size_t i=0; i < num_t; ++i) { - // calculate transect - BoostPoint v1{x0, y0 + i*distance.value()}; - BoostPoint v2{x1, y0 + i*distance.value()}; - BoostLineString transect; - transect.push_back(v1); - transect.push_back(v2); - - // transform back - BoostLineString temp_transect; - bg::transform(transect, temp_transect, rotate_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); - } - - if (transectsClipper.size() == 0){ - std::stringstream ss; - ss << "Not able to generate transects. Parameter: distance = " << distance << std::endl; - errorString = ss.str(); - return false; + string &errorString) { + // Rotate measurement area by angle and calculate bounding box. + auto &mArea = scenario.measurementArea(); + BoostPolygon mAreaRotated; + trans::rotate_transformer rotate(-angle.value() * + 180 / M_PI); + bg::transform(mArea, mAreaRotated, rotate); + + BoostBox box; + boost::geometry::envelope(mAreaRotated, box); + + double x0 = box.min_corner().get<0>(); + double y0 = box.min_corner().get<1>(); + double x1 = box.max_corner().get<0>(); + double y1 = box.max_corner().get<1>(); + + // Generate transects and convert them to clipper path. + size_t num_t = int(ceil((y1 - y0) / distance.value())); // number of transects + trans::rotate_transformer rotate_back( + angle.value() * 180 / M_PI); + vector transectsClipper; + transectsClipper.reserve(num_t); + for (size_t i = 0; i < num_t; ++i) { + // calculate transect + BoostPoint v1{x0, y0 + i * distance.value()}; + BoostPoint v2{x1, y0 + i * distance.value()}; + BoostLineString transect; + transect.push_back(v1); + transect.push_back(v2); + + // transform back + BoostLineString temp_transect; + bg::transform(transect, temp_transect, rotate_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); + } + + if (transectsClipper.size() == 0) { + std::stringstream ss; + ss << "Not able to generate transects. Parameter: distance = " << distance + << std::endl; + errorString = ss.str(); + return false; + } + + // Convert measurement area to clipper path. + ClipperLib::Path mAreaClipper; + for (auto vertex : mArea.outer()) { + mAreaClipper.push_back(ClipperLib::IntPoint{ + static_cast(vertex.get<0>() * CLIPPER_SCALE), + static_cast(vertex.get<1>() * CLIPPER_SCALE)}); + } + + // Perform clipping. + // Clip transects to measurement area. + ClipperLib::Clipper clipper; + clipper.AddPath(mAreaClipper, ClipperLib::ptClip, true); + clipper.AddPaths(transectsClipper, ClipperLib::ptSubject, false); + ClipperLib::PolyTree clippedTransecs; + clipper.Execute(ClipperLib::ctIntersection, clippedTransecs, + ClipperLib::pftNonZero, ClipperLib::pftNonZero); + auto &transects = clippedTransecs; + + bool ignoreProgress = p.size() != scenario.tiles().size(); + ClipperLib::PolyTree clippedTransecs2; + if (!ignoreProgress) { + // Calculate processed tiles (_progress[i] == 100) and subtract them from + // measurement area. + size_t numTiles = p.size(); + vector processedTiles; + const auto &tiles = scenario.tiles(); + for (size_t i = 0; i < numTiles; ++i) { + if (p[i] == 100) { + processedTiles.push_back(tiles[i]); + } } - // Convert measurement area to clipper path. - ClipperLib::Path mAreaClipper; - for ( auto vertex : mArea.outer() ){ - mAreaClipper.push_back(ClipperLib::IntPoint{static_cast(vertex.get<0>()*CLIPPER_SCALE), - static_cast(vertex.get<1>()*CLIPPER_SCALE)}); - } - - // Perform clipping. - // Clip transects to measurement area. - ClipperLib::Clipper clipper; - clipper.AddPath(mAreaClipper, ClipperLib::ptClip, true); - clipper.AddPaths(transectsClipper, ClipperLib::ptSubject, false); - ClipperLib::PolyTree clippedTransecs; - clipper.Execute(ClipperLib::ctIntersection, clippedTransecs, ClipperLib::pftNonZero, ClipperLib::pftNonZero); - auto &transects = clippedTransecs; - - bool ignoreProgress = p.size() != scenario.tiles().size(); - ClipperLib::PolyTree clippedTransecs2; - if ( !ignoreProgress ) { - // Calculate processed tiles (_progress[i] == 100) and subtract them from measurement area. - size_t numTiles = p.size(); - vector processedTiles; - const auto &tiles = scenario.tiles(); - for (size_t i=0; i 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); - } - - // Subtract holes (tiles with measurement_progress == 100) from transects. - clipper.Clear(); - for (auto &child : clippedTransecs.Childs) - clipper.AddPath(child->Contour, ClipperLib::ptSubject, false); - clipper.AddPaths(processedTilesClipper, ClipperLib::ptClip, true); - clipper.Execute(ClipperLib::ctDifference, clippedTransecs2, ClipperLib::pftNonZero, ClipperLib::pftNonZero); - transects = clippedTransecs2; + if (processedTiles.size() != numTiles) { + 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); + } + + // Subtract holes (tiles with measurement_progress == 100) from transects. + clipper.Clear(); + for (auto &child : clippedTransecs.Childs) + clipper.AddPath(child->Contour, ClipperLib::ptSubject, false); + clipper.AddPaths(processedTilesClipper, ClipperLib::ptClip, true); + clipper.Execute(ClipperLib::ctDifference, clippedTransecs2, + ClipperLib::pftNonZero, ClipperLib::pftNonZero); + transects = clippedTransecs2; } - - // Extract transects from PolyTree and convert them to BoostLineString - for (auto &child : transects.Childs){ - auto &clipperTransect = child->Contour; - BoostPoint v1{static_cast(clipperTransect[0].X)/CLIPPER_SCALE, - static_cast(clipperTransect[0].Y)/CLIPPER_SCALE}; - BoostPoint v2{static_cast(clipperTransect[1].X)/CLIPPER_SCALE, - static_cast(clipperTransect[1].Y)/CLIPPER_SCALE}; - - BoostLineString transect{v1, v2}; - if (bg::length(transect) >= minLength.value()) - t.push_back(transect); - } - - if (t.size() == 0){ - std::stringstream ss; - ss << "Not able to generate transects. Parameter: minLength = " << minLength << std::endl; - errorString = ss.str(); - return false; - } - return true; + } + + // Extract transects from PolyTree and convert them to BoostLineString + for (auto &child : transects.Childs) { + auto &clipperTransect = child->Contour; + BoostPoint v1{static_cast(clipperTransect[0].X) / CLIPPER_SCALE, + static_cast(clipperTransect[0].Y) / CLIPPER_SCALE}; + BoostPoint v2{static_cast(clipperTransect[1].X) / CLIPPER_SCALE, + static_cast(clipperTransect[1].Y) / CLIPPER_SCALE}; + + BoostLineString transect{v1, v2}; + if (bg::length(transect) >= minLength.value()) + t.push_back(transect); + } + + if (t.size() == 0) { + std::stringstream ss; + ss << "Not able to generate transects. Parameter: minLength = " << minLength + << std::endl; + errorString = ss.str(); + return false; + } + return true; } - - -struct RoutingDataModel{ - Matrix distanceMatrix; - long numVehicles; - RoutingIndexManager::NodeIndex depot; +struct RoutingDataModel { + Matrix distanceMatrix; + long numVehicles; + RoutingIndexManager::NodeIndex depot; }; void generateRoutingModel(const BoostLineString &vertices, - const BoostPolygon &polygonOffset, - size_t n0, - RoutingDataModel &dataModel, - Matrix &graph){ + const BoostPolygon &polygonOffset, size_t n0, + RoutingDataModel &dataModel, Matrix &graph) { #ifdef SHOW_TIME - auto start = std::chrono::high_resolution_clock::now(); + auto start = std::chrono::high_resolution_clock::now(); #endif - graphFromPolygon(polygonOffset, vertices, graph); + 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; + auto delta = std::chrono::duration_cast( + std::chrono::high_resolution_clock::now() - start); + cout << "Execution time graphFromPolygon(): " << delta.count() << " ms" + << endl; #endif - Matrix distanceMatrix(graph); + Matrix distanceMatrix(graph); #ifdef SHOW_TIME - start = std::chrono::high_resolution_clock::now(); + start = std::chrono::high_resolution_clock::now(); #endif - toDistanceMatrix(distanceMatrix); + 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; + 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= 2){ - n0 += 2; - } else { - n0 += 1; - } - } - vertices.reserve(n0); - struct TransectInfo{ - TransectInfo(size_t n, bool f) - : index(n) - , front(f) - {} - - size_t index; - bool front; - }; - std::vector transectInfoList; - size_t idx = 0; - for ( size_t i=0; i= 2){ - vertices.push_back(t.back()); - transectInfoList.push_back(TransectInfo{i, false}); - } - } - - for (long i=0; i= 2) { + n0 += 2; + } else { + n0 += 1; } - for (auto &ring : area.inners()) { - for (auto &vertex : ring) - vertices.push_back(vertex); + } + vertices.reserve(n0); + struct TransectInfo { + TransectInfo(size_t n, bool f) : index(n), front(f) {} + + size_t index; + bool front; + }; + std::vector transectInfoList; + size_t idx = 0; + 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}); } - size_t n1 = vertices.size(); - // Generate routing model. - RoutingDataModel dataModel; - Matrix connectionGraph(n1, n1); - // Offset joined area. - BoostPolygon areaOffset; - offsetPolygon(area, areaOffset, detail::offsetConstant); + } + + 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 SHOW_TIME - start = std::chrono::high_resolution_clock::now(); + start = std::chrono::high_resolution_clock::now(); #endif - generateRoutingModel(vertices, areaOffset, n0, dataModel, connectionGraph); + generateRoutingModel(vertices, areaOffset, 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; + 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). - Solver *solver = routing.solver(); - size_t index = 0; - for (size_t i=0; i= 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(); - solver->AddConstraint(c); - index += 2; - } else { - index += 1; - } + // 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). + Solver *solver = routing.solver(); + size_t index = 0; + for (size_t i = 0; i < transects.size(); ++i) { + const auto &t = transects[i]; + 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(); + solver->AddConstraint(c); + 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. + } + + // 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(); + start = std::chrono::high_resolution_clock::now(); #endif - const Assignment* solution = routing.SolveWithParameters(searchParameters); + 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; + 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; + 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()); - 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){ + // 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= 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); + 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); } + } } } // namespace snake + +bool boost::geometry::model::operator==(snake::BoostPoint p1, + snake::BoostPoint p2) { + return (p1.get<0>() == p2.get<0>()) && (p1.get<1>() == p2.get<1>()); +} + +bool boost::geometry::model::operator!=(snake::BoostPoint p1, + snake::BoostPoint p2) { + return !(p1 == p2); +} diff --git a/src/Snake/snake.h b/src/Snake/snake.h index a0fd26cd908846857fccd5a8771e9d4594625ea1..5e4704edef9892c11ca028b3e90791fedc17b5c6 100644 --- a/src/Snake/snake.h +++ b/src/Snake/snake.h @@ -1,11 +1,19 @@ #pragma once -#include -#include #include #include +#include +#include + +#include +#include +#include +#include +#include +#include -#include "snake_typedefs.h" +namespace bg = boost::geometry; +namespace bu = boost::units; #include #include @@ -13,256 +21,239 @@ using namespace std; namespace snake { - //========================================================================= // Geometry stuff. //========================================================================= -template -class Matrix{ + +typedef bg::model::point BoostPoint; +// typedef std::vector BoostPointList; +typedef bg::model::polygon BoostPolygon; +typedef bg::model::linestring BoostLineString; +typedef std::vector> Int64Matrix; +typedef bg::model::box BoostBox; + +template class Matrix { public: - Matrix() : - _elements(0), - _m(0), - _n(0), - _isInitialized(false) - { - } - Matrix(size_t m, size_t n) : Matrix(m, n, T{0}){} - Matrix(size_t m, size_t n, T value) : - _elements(m*n), - _m(m), - _n(n), - _isInitialized(true) - { - assert((m > 0) || (n > 0)); - _matrix.resize(_elements, value); - } - - double get(size_t i, size_t j) const - { - assert(_isInitialized); - assert(i < _m); - assert(j < _n); - return _matrix[i*_m+j]; - } - - size_t getM() const { return _m;} - size_t getN() const { return _n;} - - void set(size_t i, size_t j, const T &value) - { - assert(_isInitialized); - assert(i < _m); - assert(j < _n); - _matrix[i*_m+j] = value; - } - - void setDimension(size_t m, size_t n, const T &value) - { - assert((m > 0) || (n > 0)); - assert(!_isInitialized); - _m = m; - _n = n; - _elements = n*m; - _matrix.resize(_elements, value); - _isInitialized = true; - } - - void setDimension(size_t m, size_t n) - { - setDimension(m, n, T{0}); - } + Matrix() : _elements(0), _m(0), _n(0), _isInitialized(false) {} + Matrix(size_t m, size_t n) : Matrix(m, n, T{0}) {} + Matrix(size_t m, size_t n, T value) + : _elements(m * n), _m(m), _n(n), _isInitialized(true) { + assert((m > 0) || (n > 0)); + _matrix.resize(_elements, value); + } + + double get(size_t i, size_t j) const { + assert(_isInitialized); + assert(i < _m); + assert(j < _n); + return _matrix[i * _m + j]; + } + + size_t getM() const { return _m; } + size_t getN() const { return _n; } + + void set(size_t i, size_t j, const T &value) { + assert(_isInitialized); + assert(i < _m); + assert(j < _n); + _matrix[i * _m + j] = value; + } + + void setDimension(size_t m, size_t n, const T &value) { + assert((m > 0) || (n > 0)); + assert(!_isInitialized); + _m = m; + _n = n; + _elements = n * m; + _matrix.resize(_elements, value); + _isInitialized = true; + } + + void setDimension(size_t m, size_t n) { setDimension(m, n, T{0}); } private: - size_t _elements; - size_t _m; - size_t _n; - bool _isInitialized; - std::vector _matrix; + size_t _elements; + size_t _m; + size_t _n; + bool _isInitialized; + std::vector _matrix; }; -struct BoundingBox{ - BoundingBox(); +struct BoundingBox { + BoundingBox(); - void clear(); + void clear(); - double width; - double height; - double angle; - BoostPolygon corners; + double width; + double height; + double angle; + BoostPolygon corners; }; -template -void toENU(const GeoPoint &origin, const GeoPoint &in, BoostPoint &out) -{ - GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), - GeographicLib::Constants::WGS84_f()); - GeographicLib::LocalCartesian proj(origin.latitude(), - origin.longitude(), - origin.altitude(), - earth); - - double x, y, z; - proj.Forward(in.latitude(), in.longitude(), in.altitude(), x, y, z); - out.set<0>(x); - out.set<0>(y); - (void)z; +template +void toENU(const GeoPoint &origin, const GeoPoint &in, BoostPoint &out) { + GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), + GeographicLib::Constants::WGS84_f()); + GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(), + origin.altitude(), earth); + + double x, y, z; + proj.Forward(in.latitude(), in.longitude(), in.altitude(), x, y, z); + out.set<0>(x); + out.set<0>(y); + (void)z; } -template -void fromENU(const GeoPoint &origin, const BoostPoint &in, GeoPoint &out) -{ - GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), - GeographicLib::Constants::WGS84_f()); - GeographicLib::LocalCartesian proj(origin.latitude(), - origin.longitude(), - origin.altitude(), - earth); - - double lat, lon, alt; - proj.Reverse(in.get<0>(), in.get<1>(), 0.0, lat, lon, alt); - out.setLatitude(lat); - out.setLongitude(lon); - out.setAltitude(alt); +template +void fromENU(const GeoPoint &origin, const BoostPoint &in, GeoPoint &out) { + GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), + GeographicLib::Constants::WGS84_f()); + GeographicLib::LocalCartesian proj(origin.latitude(), origin.longitude(), + origin.altitude(), earth); + + double lat, lon, alt; + proj.Reverse(in.get<0>(), in.get<1>(), 0.0, lat, lon, alt); + out.setLatitude(lat); + out.setLongitude(lon); + out.setAltitude(alt); } -void polygonCenter(const BoostPolygon &polygon, - BoostPoint ¢er); -void minimalBoundingBox(const BoostPolygon &polygon, - BoundingBox &minBBox); -void offsetPolygon(const BoostPolygon &polygon, - BoostPolygon &polygonOffset, - double offset); - -void graphFromPolygon(const BoostPolygon &polygon, - const BoostLineString &vertices, - Matrix &graph); +void polygonCenter(const BoostPolygon &polygon, BoostPoint ¢er); +void minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox); +void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset, + double offset); + +void graphFromPolygon(const BoostPolygon &polygon, + const BoostLineString &vertices, Matrix &graph); void toDistanceMatrix(Matrix &graph); -bool dijkstraAlgorithm(const size_t numElements, - size_t startIndex, - size_t endIndex, - std::vector &elementPath, - std::function distanceDij); -void shortestPathFromGraph(const Matrix &graph, - size_t startIndex, - size_t endIndex, - std::vector &pathIdx); +bool dijkstraAlgorithm( + const size_t numElements, size_t startIndex, size_t endIndex, + std::vector &elementPath, + std::function distanceDij); +void shortestPathFromGraph(const Matrix &graph, size_t startIndex, + size_t endIndex, std::vector &pathIdx); //========================================================================= // Scenario. //========================================================================= -class Scenario{ - public: - Scenario(); - void setMeasurementArea (const BoostPolygon &area); - void setServiceArea (const BoostPolygon &area); - void setCorridor (const BoostPolygon &area); +typedef bu::quantity Length; +typedef bu::quantity Area; +typedef bu::quantity Angle; - const BoundingBox &mAreaBoundingBox() const; +class Scenario { +public: + Scenario(); + + void setMeasurementArea(const BoostPolygon &area); + void setServiceArea(const BoostPolygon &area); + void setCorridor(const BoostPolygon &area); - const BoostPolygon &measurementArea() const; - const BoostPolygon &serviceArea()const; - const BoostPolygon &corridor()const; + const BoundingBox &mAreaBoundingBox() const; - BoostPolygon &measurementArea(); - BoostPolygon &serviceArea(); - BoostPolygon &corridor(); + const BoostPolygon &measurementArea() const; + const BoostPolygon &serviceArea() const; + const BoostPolygon &corridor() const; - Length tileWidth() const; - void setTileWidth(Length tileWidth); + BoostPolygon &measurementArea(); + BoostPolygon &serviceArea(); + BoostPolygon &corridor(); - Length tileHeight() const; - void setTileHeight(Length tileHeight); + Length tileWidth() const; + void setTileWidth(Length tileWidth); - Area minTileArea() const; - void setMinTileArea(Area minTileArea); + Length tileHeight() const; + void setTileHeight(Length tileHeight); - const BoostPolygon &joinedArea() const; - const vector &tiles() const; - const BoostLineString &tileCenterPoints() const; - const BoundingBox &measurementAreaBBox() const; - const BoostPoint &homePositon() const; + Area minTileArea() const; + void setMinTileArea(Area minTileArea); - bool update(); + const BoostPolygon &joinedArea() const; + const vector &tiles() const; + const BoostLineString &tileCenterPoints() const; + const BoundingBox &measurementAreaBBox() const; + const BoostPoint &homePositon() const; - string errorString; + bool update(); + + string errorString; private: - bool _calculateBoundingBox(); - bool _calculateTiles(); - bool _calculateJoinedArea(); + bool _calculateBoundingBox(); + bool _calculateTiles(); + bool _calculateJoinedArea(); - Length _tileWidth; - Length _tileHeight; - Area _minTileArea; + Length _tileWidth; + Length _tileHeight; + Area _minTileArea; - mutable bool _needsUpdate; + mutable bool _needsUpdate; - BoostPolygon _mArea; - BoostPolygon _sArea; - BoostPolygon _corridor; - BoostPolygon _jArea; + BoostPolygon _mArea; + BoostPolygon _sArea; + BoostPolygon _corridor; + BoostPolygon _jArea; - BoundingBox _mAreaBoundingBox; - vector _tiles; - BoostLineString _tileCenterPoints; - BoostPoint _homePosition; + BoundingBox _mAreaBoundingBox; + vector _tiles; + BoostLineString _tileCenterPoints; + BoostPoint _homePosition; }; -template class Container> -void areaToEnu( const GeoPoint &origin, - const Container &in, - BoostPolygon &out ) -{ - for(auto vertex : in) { - BoostPoint p; - toENU(origin, vertex, p); - out.outer().push_back(p); - } - bg::correct(out); +template class Container> +void areaToEnu(const GeoPoint &origin, const Container &in, + BoostPolygon &out) { + for (auto vertex : in) { + BoostPoint p; + toENU(origin, vertex, p); + out.outer().push_back(p); + } + bg::correct(out); } -template class Container> -void areaFromEnu( const GeoPoint &origin, - BoostPolygon &in, - const Container &out ) -{ - for(auto vertex : in.outer()) { - GeoPoint p; - fromENU(origin, vertex, p); - out.push_back(p); - } +template class Container> +void areaFromEnu(const GeoPoint &origin, BoostPolygon &in, + const Container &out) { + for (auto vertex : in.outer()) { + GeoPoint p; + fromENU(origin, vertex, p); + out.push_back(p); + } } -bool joinAreas(const std::vector &areas, - BoostPolygon &joinedArea); +bool joinAreas(const std::vector &areas, + BoostPolygon &joinedArea); //======================================================================================== // flight_plan //======================================================================================== -namespace flight_plan{ - - using Transects = vector; - using Progress = vector; - using Route = vector; - - bool transectsFromScenario(Length distance, - Length minLength, - Angle angle, - const Scenario &scenario, - const Progress &p, - Transects &t, - string &errorString); - bool route(const BoostPolygon &area, - const Transects &transects, - Transects &transectsRouted, - Route &route, - string &errorString); +namespace flight_plan { + +using Transects = vector; +using Progress = vector; +using Route = vector; + +bool transectsFromScenario(Length distance, Length minLength, Angle angle, + const Scenario &scenario, const Progress &p, + Transects &t, string &errorString); +bool route(const BoostPolygon &area, const Transects &transects, + Transects &transectsRouted, Route &route, string &errorString); } // namespace flight_plan namespace detail { - const double offsetConstant = 0.1; // meter, polygon offset to compenstate for numerical inaccurracies. +const double offsetConstant = + 0.1; // meter, polygon offset to compenstate for numerical inaccurracies. } // namespace detail -} +} // namespace snake + +// operator== and operator!= for boost point +namespace boost { +namespace geometry { +namespace model { + +bool operator==(snake::BoostPoint p1, snake::BoostPoint p2); +bool operator!=(snake::BoostPoint p1, snake::BoostPoint p2); +} // namespace model +} // namespace geometry +} // namespace boost diff --git a/src/Wima/Snake/SnakeDataManager.cc b/src/Wima/Snake/SnakeDataManager.cc index ab0d1c924c5b123fa44e2c086125e1703d1afc21..0ebc71ef2f8dd8cfa32e6f302d5322fea5b1574e 100644 --- a/src/Wima/Snake/SnakeDataManager.cc +++ b/src/Wima/Snake/SnakeDataManager.cc @@ -4,32 +4,30 @@ #include #include "QGCApplication.h" -#include "SettingsManager.h" #include "QGCToolbox.h" -#include "WimaSettings.h" #include "SettingsFact.h" +#include "SettingsManager.h" +#include "WimaSettings.h" #include -#include #include +#include -#include "snake.h" #include "Wima/Snake/SnakeTiles.h" #include "Wima/Snake/SnakeTilesLocal.h" +#include "snake.h" -#include "ros_bridge/include/ros_bridge.h" -#include "ros_bridge/rapidjson/include/rapidjson/document.h" -#include "ros_bridge/rapidjson/include/rapidjson/writer.h" -#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.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/progress.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 - +#define TIMEOUT 3000 // ms using QVariantList = QList; using ROSBridgePtr = std::unique_ptr; @@ -38,549 +36,552 @@ using UniqueLock = std::unique_lock; using SharedLock = std::shared_lock; using JsonDocUPtr = ros_bridge::com_private::JsonDocUPtr; -class SnakeImpl : public QObject{ - Q_OBJECT +class SnakeDataManager::SnakeImpl { public: - SnakeImpl(SnakeDataManager* p); - - bool precondition() const; - void resetWaypointData(); - 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; - 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; + SnakeImpl(SnakeDataManager *p) + : rosBridgeEnabeled(false), topicServiceSetupDone(false), + lineDistance(1 * si::meter), minTransectLength(1 * si::meter), + calcInProgress(false), parent(p) { + // ROS Bridge. + WimaSettings *wimaSettings = + qgcApp()->toolbox()->settingsManager()->wimaSettings(); + auto connectionStringFact = wimaSettings->rosbridgeConnectionString(); + auto setConnectionString = [connectionStringFact, this] { + auto connectionString = connectionStringFact->rawValue().toString(); + if (ros_bridge::isValidConnectionString( + connectionString.toLocal8Bit().data())) { + this->pRosBridge.reset( + new ros_bridge::ROSBridge(connectionString.toLocal8Bit().data())); + } else { + QString defaultString("localhost:9090"); + qgcApp()->showMessage("ROS Bridge connection string invalid: " + + connectionString); + qgcApp()->showMessage("Resetting connection string to: " + + defaultString); + connectionStringFact->setRawValue( + QVariant(defaultString)); // calls this function recursively + } + }; + connect(connectionStringFact, &SettingsFact::rawValueChanged, + setConnectionString); + setConnectionString(); + // Periodic. + connect(&this->eventTimer, &QTimer::timeout, [this] { + if (this->rosBridgeEnabeled) { + if (!this->pRosBridge->isRunning()) { + this->pRosBridge->start(); + } else if (this->pRosBridge->isRunning() && + this->pRosBridge->connected() && + !this->topicServiceSetupDone) { + if (this->doTopicServiceSetup()) + this->topicServiceSetupDone = true; + } else if (this->pRosBridge->isRunning() && + !this->pRosBridge->connected() && + this->topicServiceSetupDone) { + this->pRosBridge->reset(); + this->pRosBridge->start(); + this->topicServiceSetupDone = false; + } + } else if (this->pRosBridge->isRunning()) { + this->pRosBridge->reset(); + this->topicServiceSetupDone = false; + } + }); + this->eventTimer.start(EVENT_TIMER_INTERVAL); + } + + 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; }; -template -class ToggleRAII{ -public: - ToggleRAII(AtomicType &t) - : _t(t) - {} - - ~ToggleRAII() - { - if ( _t.load() ){ - _t.store(false); - } else { - _t.store(true); +bool SnakeDataManager::SnakeImpl::doTopicServiceSetup() { + using namespace ros_bridge::messages; + UniqueLock lk(this->mutex); + + if (this->tilesENU.polygons().size() == 0) + return false; + + // Publish snake origin. + this->pRosBridge->advertiseTopic( + "/snake/origin", geographic_msgs::geo_point::messageType().c_str()); + JsonDocUPtr jOrigin( + std::make_unique(rapidjson::kObjectType)); + bool ret = geographic_msgs::geo_point::toJson(this->ENUOrigin, *jOrigin, + jOrigin->GetAllocator()); + Q_ASSERT(ret); + (void)ret; + this->pRosBridge->publish(std::move(jOrigin), "/snake/origin"); + + // Publish snake tiles. + this->pRosBridge->advertiseTopic( + "/snake/tiles", + jsk_recognition_msgs::polygon_array::messageType().c_str()); + JsonDocUPtr jSnakeTiles( + std::make_unique(rapidjson::kObjectType)); + ret = jsk_recognition_msgs::polygon_array::toJson( + this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator()); + Q_ASSERT(ret); + (void)ret; + this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles"); + + // Subscribe nemo progress. + this->pRosBridge->subscribe( + "/nemo/progress", + /* callback */ [this](JsonDocUPtr pDoc) { + UniqueLock lk(this->mutex); + int requiredSize = this->tilesENU.polygons().size(); + auto &progressMsg = this->qProgress; + if (!nemo_msgs::progress::fromJson(*pDoc, progressMsg) || + progressMsg.progress().size() != + requiredSize) { // Some error occured. + // Set progress to default. + progressMsg.progress().fill(0, requiredSize); + + // Publish snake origin. + JsonDocUPtr jOrigin( + std::make_unique(rapidjson::kObjectType)); + bool ret = geographic_msgs::geo_point::toJson( + this->ENUOrigin, *jOrigin, jOrigin->GetAllocator()); + Q_ASSERT(ret); + (void)ret; + this->pRosBridge->publish(std::move(jOrigin), "/snake/origin"); + + // Publish snake tiles. + JsonDocUPtr jSnakeTiles( + std::make_unique(rapidjson::kObjectType)); + ret = jsk_recognition_msgs::polygon_array::toJson( + this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator()); + Q_ASSERT(ret); + (void)ret; + this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles"); } - } -private: - AtomicType &_t; -}; + this->progress.clear(); + for (const auto &p : progressMsg.progress()) { + this->progress.push_back(p); + } + lk.unlock(); + + emit this->parent->nemoProgressChanged(); + }); + + // Subscribe /nemo/heartbeat. + this->pRosBridge->subscribe( + "/nemo/heartbeat", + /* callback */ [this](JsonDocUPtr pDoc) { + UniqueLock lk(this->mutex); + if (this->timeout.isActive()) { + this->timeout.stop(); + } + auto &heartbeatMsg = this->heartbeat; + if (!nemo_msgs::heartbeat::fromJson(*pDoc, heartbeatMsg)) { + heartbeatMsg.setStatus(integral(NemoStatus::InvalidHeartbeat)); + } + this->timeout.singleShot(TIMEOUT, [this] { + UniqueLock lk(this->mutex); + this->heartbeat.setStatus(integral(NemoStatus::Timeout)); + emit this->parent->nemoStatusChanged(integral(NemoStatus::Timeout)); + }); + emit this->parent->nemoStatusChanged(heartbeatMsg.status()); + }); + // Advertise /snake/get_origin. + this->pRosBridge->advertiseService( + "/snake/get_origin", "snake_msgs/GetOrigin", + [this](JsonDocUPtr) -> JsonDocUPtr { + using namespace ros_bridge::messages; + SharedLock lk(this->mutex); + + JsonDocUPtr pDoc( + std::make_unique(rapidjson::kObjectType)); + auto &origin = this->ENUOrigin; + rapidjson::Value jOrigin(rapidjson::kObjectType); + bool ret = geographic_msgs::geo_point::toJson(origin, jOrigin, + pDoc->GetAllocator()); + lk.unlock(); + Q_ASSERT(ret); + (void)ret; + pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator()); + return pDoc; + }); + // Advertise /snake/get_tiles. + this->pRosBridge->advertiseService( + "/snake/get_tiles", "snake_msgs/GetTiles", + [this](JsonDocUPtr) -> JsonDocUPtr { + SharedLock lk(this->mutex); -SnakeDataManager::SnakeDataManager(QObject *parent) - : QThread(parent) - , pImpl(std::make_unique(this)) + JsonDocUPtr pDoc( + std::make_unique(rapidjson::kObjectType)); + rapidjson::Value jSnakeTiles(rapidjson::kObjectType); + bool ret = jsk_recognition_msgs::polygon_array::toJson( + this->tilesENU, jSnakeTiles, pDoc->GetAllocator()); + Q_ASSERT(ret); + (void)ret; + pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator()); + return pDoc; + }); -{ + return true; } -SnakeDataManager::~SnakeDataManager() -{ +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(); } -void SnakeDataManager::setMeasurementArea(const QList &measurementArea) -{ - UniqueLock lk(this->pImpl->mutex); - this->pImpl->mArea = measurementArea; -} +template class CommandRAII { +public: + CommandRAII(Callable &fun) : _fun(fun) {} + + ~CommandRAII() { _fun(); } -void SnakeDataManager::setServiceArea(const QList &serviceArea) -{ - UniqueLock lk(this->pImpl->mutex); - this->pImpl->sArea = serviceArea; +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; } -void SnakeDataManager::setCorridor(const QList &corridor) -{ - UniqueLock lk(this->pImpl->mutex); - this->pImpl->corridor = corridor; +void SnakeDataManager::setServiceArea( + const QList &serviceArea) { + UniqueLock lk(this->pImpl->mutex); + this->pImpl->sArea = serviceArea; } -QNemoProgress SnakeDataManager::nemoProgress() -{ - SharedLock lk(this->pImpl->mutex); - return this->pImpl->qProgress; +void SnakeDataManager::setCorridor(const QList &corridor) { + UniqueLock lk(this->pImpl->mutex); + this->pImpl->corridor = corridor; } -int SnakeDataManager::nemoStatus() -{ - SharedLock lk(this->pImpl->mutex); - return this->pImpl->heartbeat.status(); +const QmlObjectListModel *SnakeDataManager::tiles() const { + SharedLock lk(this->pImpl->mutex); + return &this->pImpl->tilesQml; } -bool SnakeDataManager::calcInProgress() -{ - return this->pImpl->calcInProgress.load(); +QVariantList SnakeDataManager::tileCenterPoints() const { + SharedLock lk(this->pImpl->mutex); + return this->pImpl->tileCenterPoints; } -Length SnakeDataManager::lineDistance() const -{ - SharedLock lk(this->pImpl->mutex); - return this->pImpl->lineDistance; +QNemoProgress SnakeDataManager::nemoProgress() const { + SharedLock lk(this->pImpl->mutex); + return this->pImpl->qProgress; } -void SnakeDataManager::setLineDistance(Length lineDistance) -{ - UniqueLock lk(this->pImpl->mutex); - this->pImpl->lineDistance = lineDistance; +int SnakeDataManager::nemoStatus() const { + SharedLock lk(this->pImpl->mutex); + return this->pImpl->heartbeat.status(); } -Area SnakeDataManager::minTileArea() const -{ - SharedLock lk(this->pImpl->mutex); - return this->pImpl->scenario.minTileArea(); +bool SnakeDataManager::calcInProgress() const { + SharedLock lk(this->pImpl->mutex); + return this->pImpl->calcInProgress.load(); } -void SnakeDataManager::setMinTileArea(Area minTileArea) -{ - UniqueLock lk(this->pImpl->mutex); - this->pImpl->scenario.setMinTileArea(minTileArea); +QString SnakeDataManager::errorMessage() const { + SharedLock lk(this->pImpl->mutex); + return this->pImpl->errorMessage; } -Length SnakeDataManager::tileHeight() const -{ - SharedLock lk(this->pImpl->mutex); - return this->pImpl->scenario.tileHeight(); +bool SnakeDataManager::success() const { + SharedLock lk(this->pImpl->mutex); + return this->pImpl->errorMessage.isEmpty() ? true : false; } -void SnakeDataManager::setTileHeight(Length tileHeight) -{ - UniqueLock lk(this->pImpl->mutex); - this->pImpl->scenario.setTileHeight(tileHeight); +QVector SnakeDataManager::waypoints() const { + SharedLock lk(this->pImpl->mutex); + return this->pImpl->waypoints; } -Length SnakeDataManager::tileWidth() const -{ - SharedLock lk(this->pImpl->mutex); - return this->pImpl->scenario.tileWidth(); +QVector SnakeDataManager::arrivalPath() const { + SharedLock lk(this->pImpl->mutex); + return this->pImpl->arrivalPath; } -void SnakeDataManager::setTileWidth(Length tileWidth) -{ - UniqueLock lk(this->pImpl->mutex); - this->pImpl->scenario.setTileWidth(tileWidth); +QVector SnakeDataManager::returnPath() const { + SharedLock lk(this->pImpl->mutex); + return this->pImpl->returnPath; } -void SnakeDataManager::enableRosBridge() -{ - this->pImpl->rosBridgeEnabeled = true; +Length SnakeDataManager::lineDistance() const { + SharedLock lk(this->pImpl->mutex); + return this->pImpl->lineDistance; } -void SnakeDataManager::disableRosBride() -{ - this->pImpl->rosBridgeEnabeled = false; +void SnakeDataManager::setLineDistance(Length lineDistance) { + UniqueLock lk(this->pImpl->mutex); + this->pImpl->lineDistance = lineDistance; } -bool SnakeImpl::precondition() const -{ - return true; +Area SnakeDataManager::minTileArea() const { + SharedLock lk(this->pImpl->mutex); + return this->pImpl->scenario.minTileArea(); } -//! -//! \brief Resets waypoint data. -//! \pre this->_pImpl->mutex must be locked. -void SnakeImpl::resetWaypointData() -{ - 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->tiles.polygons().clear(); - this->tileCenterPoints.clear(); - this->tilesENU.polygons().clear(); - this->tileCenterPointsENU.clear(); - this->errorMessage.clear(); +void SnakeDataManager::setMinTileArea(Area minTileArea) { + UniqueLock lk(this->pImpl->mutex); + this->pImpl->scenario.setMinTileArea(minTileArea); } -void SnakeDataManager::run() -{ - this->pImpl->calcInProgress.store(true); - ToggleRAII tr(this->pImpl->calcInProgress); +Length SnakeDataManager::tileHeight() const { + SharedLock lk(this->pImpl->mutex); + return this->pImpl->scenario.tileHeight(); +} - UniqueLock lk(this->pImpl->mutex); - this->pImpl->resetWaypointData(); +void SnakeDataManager::setTileHeight(Length tileHeight) { + UniqueLock lk(this->pImpl->mutex); + this->pImpl->scenario.setTileHeight(tileHeight); +} - if ( !this->pImpl->precondition() ) - return; +Length SnakeDataManager::tileWidth() const { + SharedLock lk(this->pImpl->mutex); + return this->pImpl->scenario.tileWidth(); +} - 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; - } +void SnakeDataManager::setTileWidth(Length tileWidth) { + UniqueLock lk(this->pImpl->mutex); + this->pImpl->scenario.setTileWidth(tileWidth); +} +void SnakeDataManager::enableRosBridge() { + this->pImpl->rosBridgeEnabeled = true; +} - this->pImpl->ENUOrigin = this->pImpl->mArea.front(); - auto &origin = this->pImpl->ENUOrigin; - // 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); - } +void SnakeDataManager::disableRosBride() { + this->pImpl->rosBridgeEnabeled = false; +} - // 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); +void SnakeDataManager::run() { + this->pImpl->calcInProgress.store(true); + emit calcInProgressChanged(this->pImpl->calcInProgress.load()); + auto onExit = [this] { + 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; + // 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; + } + + // Asynchronously update flightplan. + std::string errorString; + auto future = std::async([this, &errorString, &origin] { + 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 = "Not able to generate transects."; + return value; } - - // 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); + 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 = "Routing error."; + return value; } - if ( !this->pImpl->scenario.update() ){ - this->pImpl->errorMessage = this->pImpl->scenario.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); } - - // Asynchronously update flightplan. - qWarning() << "route calculation missing"; - std::string errorString; - auto future = std::async([this, &errorString, &origin]{ - 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 ) - return value; - 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 ) - return value; - - // Store arrival path. - const auto &firstWaypoint = transectsRouted.front().front(); - long startIdx = 0; - for (long i = 0; i < 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); - } - - return true; - - }); - - // Continue with storing scenario data in the mean time. - { - // 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->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); - } + // 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); } - - future.wait(); - // Trying to generate flight plan. - if ( !future.get() ){ - // error - this->pImpl->errorMessage = errorString.c_str(); - } else { - // Success!!! - + // 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); } -} - -bool SnakeImpl::doTopicServiceSetup() -{ - using namespace ros_bridge::messages; - UniqueLock lk(this->mutex); - - if ( this->tilesENU.polygons().size() == 0) - return false; - - // Publish snake origin. - this->pRosBridge->advertiseTopic("/snake/origin", - geographic_msgs::geo_point::messageType().c_str()); - JsonDocUPtr jOrigin(std::make_unique(rapidjson::kObjectType)); - bool ret = geographic_msgs::geo_point::toJson( - this->ENUOrigin, *jOrigin, jOrigin->GetAllocator()); - Q_ASSERT(ret); - (void)ret; - this->pRosBridge->publish(std::move(jOrigin), "/snake/origin"); - - - // Publish snake tiles. - this->pRosBridge->advertiseTopic("/snake/tiles", - jsk_recognition_msgs::polygon_array::messageType().c_str()); - JsonDocUPtr jSnakeTiles(std::make_unique(rapidjson::kObjectType)); - ret = jsk_recognition_msgs::polygon_array::toJson( - this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator()); - Q_ASSERT(ret); - (void)ret; - this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles"); - - - // Subscribe nemo progress. - this->pRosBridge->subscribe("/nemo/progress", - /* callback */ [this](JsonDocUPtr pDoc){ - UniqueLock lk(this->mutex); - int requiredSize = this->tilesENU.polygons().size(); - auto& progressMsg = this->qProgress; - if ( !nemo_msgs::progress::fromJson(*pDoc, progressMsg) - || progressMsg.progress().size() != requiredSize ) { // Some error occured. - // Set progress to default. - progressMsg.progress().fill(0, requiredSize); - - // Publish snake origin. - JsonDocUPtr jOrigin(std::make_unique(rapidjson::kObjectType)); - bool ret = geographic_msgs::geo_point::toJson( - this->ENUOrigin, *jOrigin, jOrigin->GetAllocator()); - Q_ASSERT(ret); - (void)ret; - this->pRosBridge->publish(std::move(jOrigin), "/snake/origin"); - - // Publish snake tiles. - JsonDocUPtr jSnakeTiles(std::make_unique(rapidjson::kObjectType)); - ret = jsk_recognition_msgs::polygon_array::toJson( - this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator()); - Q_ASSERT(ret); - (void)ret; - this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles"); - } - this->progress.clear(); - for (const auto&p : progressMsg.progress()){ - this->progress.push_back(p); - } - lk.unlock(); - - emit this->parent->nemoProgressChanged(); - }); - - - // Subscribe /nemo/heartbeat. - this->pRosBridge->subscribe("/nemo/heartbeat", - /* callback */ [this](JsonDocUPtr pDoc){ - UniqueLock lk(this->mutex); - if ( this->timeout.isActive() ) { - this->timeout.stop(); - } - auto &heartbeatMsg = this->heartbeat; - if ( !nemo_msgs::heartbeat::fromJson(*pDoc, heartbeatMsg) ) { - heartbeatMsg.setStatus(integral(NemoStatus::InvalidHeartbeat)); - } - this->timeout.singleShot(TIMEOUT, [this]{ - UniqueLock lk(this->mutex); - this->heartbeat.setStatus(integral(NemoStatus::Timeout)); - emit this->parent->nemoStatusChanged(integral(NemoStatus::Timeout)); - }); - emit this->parent->nemoStatusChanged(heartbeatMsg.status()); - }); - - - // Advertise /snake/get_origin. - this->pRosBridge->advertiseService("/snake/get_origin", "snake_msgs/GetOrigin", - [this](JsonDocUPtr) -> JsonDocUPtr - { - using namespace ros_bridge::messages; - SharedLock lk(this->mutex); - - JsonDocUPtr pDoc(std::make_unique(rapidjson::kObjectType)); - auto &origin = this->ENUOrigin; - rapidjson::Value jOrigin(rapidjson::kObjectType); - bool ret = geographic_msgs::geo_point::toJson( - origin, jOrigin, pDoc->GetAllocator()); - lk.unlock(); - Q_ASSERT(ret); - (void)ret; - pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator()); - return pDoc; - }); - - - // Advertise /snake/get_tiles. - this->pRosBridge->advertiseService("/snake/get_tiles", "snake_msgs/GetTiles", - [this](JsonDocUPtr) -> JsonDocUPtr - { - SharedLock lk(this->mutex); - - JsonDocUPtr pDoc(std::make_unique(rapidjson::kObjectType)); - rapidjson::Value jSnakeTiles(rapidjson::kObjectType); - bool ret = jsk_recognition_msgs::polygon_array::toJson( - this->tilesENU, jSnakeTiles, pDoc->GetAllocator()); - Q_ASSERT(ret); - (void)ret; - pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator()); - return pDoc; - }); return true; -} - - - -SnakeImpl::SnakeImpl(SnakeDataManager *p) : - rosBridgeEnabeled(false) - , topicServiceSetupDone(false) - , lineDistance(1*si::meter) - , minTransectLength(1*si::meter) - , calcInProgress(false) - , parent(p) -{ - // ROS Bridge. - WimaSettings* wimaSettings = qgcApp()->toolbox()->settingsManager()->wimaSettings(); - auto connectionStringFact = wimaSettings->rosbridgeConnectionString(); - auto setConnectionString = [connectionStringFact, this]{ - auto connectionString = connectionStringFact->rawValue().toString(); - if ( ros_bridge::isValidConnectionString(connectionString.toLocal8Bit().data()) ){ - this->pRosBridge.reset(new ros_bridge::ROSBridge(connectionString.toLocal8Bit().data())); - } else { - QString defaultString("localhost:9090"); - qgcApp()->showMessage("ROS Bridge connection string invalid: " + connectionString); - qgcApp()->showMessage("Resetting connection string to: " + defaultString); - connectionStringFact->setRawValue(QVariant(defaultString)); // calls this function recursively - } - }; - connect(connectionStringFact, &SettingsFact::rawValueChanged, setConnectionString); - setConnectionString(); - - // Periodic. - connect(&this->eventTimer, &QTimer::timeout, [this]{ - if ( this->rosBridgeEnabeled ) { - if ( !this->pRosBridge->isRunning()) { - this->pRosBridge->start(); - } else if ( this->pRosBridge->isRunning() - && this->pRosBridge->connected() - && !this->topicServiceSetupDone) { - if ( this->doTopicServiceSetup() ) - this->topicServiceSetupDone = true; - } else if ( this->pRosBridge->isRunning() - && !this->pRosBridge->connected() - && this->topicServiceSetupDone){ - this->pRosBridge->reset(); - this->pRosBridge->start(); - this->topicServiceSetupDone = false; - } - } else if ( this->pRosBridge->isRunning() ) { - this->pRosBridge->reset(); - this->topicServiceSetupDone = false; - } - }); - this->eventTimer.start(EVENT_TIMER_INTERVAL); + }); + + // Continue with storing scenario data in the mean time. + { + // 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); + } + } + + future.wait(); + // Trying to generate flight plan. + if (!future.get()) { + // error + this->pImpl->errorMessage = errorString.c_str(); + } else { + // Success!!! + } } diff --git a/src/Wima/Snake/SnakeDataManager.h b/src/Wima/Snake/SnakeDataManager.h index 795a7a3a5f85e511986f3f97aaee3535be69e168..9bc74e5983e23a3416bba313f2e2d8499a1386ad 100644 --- a/src/Wima/Snake/SnakeDataManager.h +++ b/src/Wima/Snake/SnakeDataManager.h @@ -1,81 +1,79 @@ #pragma once +#include +#include #include #include -#include -#include -#include "QNemoProgress.h" #include "QNemoHeartbeat.h" +#include "QNemoProgress.h" +#include "SnakeTiles.h" #include - using namespace boost::units; using Length = quantity; using Area = quantity; -class SnakeImpl; - -enum class NemoStatus{ - NotConnected = 0, - Connected = 1, - Timeout = -1, - InvalidHeartbeat = -2 +enum class NemoStatus { + NotConnected = 0, + Connected = 1, + Timeout = -1, + InvalidHeartbeat = -2 }; -class SnakeDataManager : public QThread{ - Q_OBJECT +class SnakeDataManager : public QThread { + Q_OBJECT -public: - using SnakeImplPtr = std::unique_ptr; + class SnakeImpl; + using SnakeImplPtr = std::unique_ptr; - SnakeDataManager(QObject *parent = nullptr); - ~SnakeDataManager() override; +public: + SnakeDataManager(QObject *parent = nullptr); + ~SnakeDataManager() override; + void setMeasurementArea(const QList &measurementArea); + void setServiceArea(const QList &serviceArea); + void setCorridor(const QList &corridor); - void setMeasurementArea (const QList &measurementArea); - void setServiceArea (const QList &serviceArea); - void setCorridor (const QList &corridor); + const QmlObjectListModel *tiles() const; + QVariantList tileCenterPoints() const; + QNemoProgress nemoProgress() const; + int nemoStatus() const; + bool calcInProgress() const; + QString errorMessage() const; + bool success() const; - QNemoProgress nemoProgress(); - int nemoStatus(); - bool calcInProgress(); + QVector waypoints() const; + QVector arrivalPath() const; + QVector returnPath() const; - Length lineDistance() const; - void setLineDistance(Length lineDistance); + Length lineDistance() const; + void setLineDistance(Length lineDistance); - Length minTransectLength() const; - void setMinTransectLength(Length minTransectLength); + Length minTransectLength() const; + void setMinTransectLength(Length minTransectLength); - Area minTileArea() const; - void setMinTileArea(Area minTileArea); + Area minTileArea() const; + void setMinTileArea(Area minTileArea); - Length tileHeight() const; - void setTileHeight(Length tileHeight); + Length tileHeight() const; + void setTileHeight(Length tileHeight); - Length tileWidth() const; - void setTileWidth(Length tileWidth); + Length tileWidth() const; + void setTileWidth(Length tileWidth); - void enableRosBridge(); - void disableRosBride(); + void enableRosBridge(); + void disableRosBride(); signals: - void nemoProgressChanged(); - void nemoStatusChanged(int status); - void calcInProgressChanged(bool inProgress); + void nemoProgressChanged(); + void nemoStatusChanged(int status); + void calcInProgressChanged(bool inProgress); protected: - void run() override; + void run() override; + private: - SnakeImplPtr pImpl; + SnakeImplPtr pImpl; }; - - - - - - - - - diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index ed259d242c2965d1942c7a9f9ddd8ea520576971..e1ab2b4528de232b1f0957903236d9af2473f977 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -2,37 +2,38 @@ #include "utilities.h" -#include "Snake/QNemoProgress.h" #include "Snake/QNemoHeartbeat.h" +#include "Snake/QNemoProgress.h" #include "QVector3D" #include +#include #include -#define SMART_RTL_MAX_ATTEMPTS 3 // times +#define SMART_RTL_MAX_ATTEMPTS 3 // times #define SMART_RTL_ATTEMPT_INTERVAL 200 // ms -#define EVENT_TIMER_INTERVAL 50 // ms - - - -const char* WimaController::areaItemsName = "AreaItems"; -const char* WimaController::missionItemsName = "MissionItems"; -const char* WimaController::settingsGroup = "WimaController"; -const char* WimaController::enableWimaControllerName = "EnableWimaController"; -const char* WimaController::overlapWaypointsName = "OverlapWaypoints"; -const char* WimaController::maxWaypointsPerPhaseName = "MaxWaypointsPerPhase"; -const char* WimaController::startWaypointIndexName = "StartWaypointIndex"; -const char* WimaController::showAllMissionItemsName = "ShowAllMissionItems"; -const char* WimaController::showCurrentMissionItemsName = "ShowCurrentMissionItems"; -const char* WimaController::flightSpeedName = "FlightSpeed"; -const char* WimaController::arrivalReturnSpeedName = "ArrivalReturnSpeed"; -const char* WimaController::altitudeName = "Altitude"; -const char* WimaController::snakeTileWidthName = "SnakeTileWidth"; -const char* WimaController::snakeTileHeightName = "SnakeTileHeight"; -const char* WimaController::snakeMinTileAreaName = "SnakeMinTileArea"; -const char* WimaController::snakeLineDistanceName = "SnakeLineDistance"; -const char* WimaController::snakeMinTransectLengthName = "SnakeMinTransectLength"; +#define EVENT_TIMER_INTERVAL 50 // ms + +const char *WimaController::areaItemsName = "AreaItems"; +const char *WimaController::missionItemsName = "MissionItems"; +const char *WimaController::settingsGroup = "WimaController"; +const char *WimaController::enableWimaControllerName = "EnableWimaController"; +const char *WimaController::overlapWaypointsName = "OverlapWaypoints"; +const char *WimaController::maxWaypointsPerPhaseName = "MaxWaypointsPerPhase"; +const char *WimaController::startWaypointIndexName = "StartWaypointIndex"; +const char *WimaController::showAllMissionItemsName = "ShowAllMissionItems"; +const char *WimaController::showCurrentMissionItemsName = + "ShowCurrentMissionItems"; +const char *WimaController::flightSpeedName = "FlightSpeed"; +const char *WimaController::arrivalReturnSpeedName = "ArrivalReturnSpeed"; +const char *WimaController::altitudeName = "Altitude"; +const char *WimaController::snakeTileWidthName = "SnakeTileWidth"; +const char *WimaController::snakeTileHeightName = "SnakeTileHeight"; +const char *WimaController::snakeMinTileAreaName = "SnakeMinTileArea"; +const char *WimaController::snakeLineDistanceName = "SnakeLineDistance"; +const char *WimaController::snakeMinTransectLengthName = + "SnakeMinTransectLength"; WimaController::StatusMap WimaController::_nemoStatusMap{ std::make_pair(0, "No Heartbeat"), @@ -40,909 +41,754 @@ WimaController::StatusMap WimaController::_nemoStatusMap{ std::make_pair(-1, "Timeout"), std::make_pair(-2, "Error")}; -using namespace snake; - WimaController::WimaController(QObject *parent) - : QObject (parent) - , _joinedArea () - , _measurementArea () - , _serviceArea () - , _corridor () - , _localPlanDataValid (false) - , _areaInterface (&_measurementArea, &_serviceArea, &_corridor, &_joinedArea) - , _managerSettings () - , _defaultManager (_managerSettings, _areaInterface) - , _snakeManager (_managerSettings, _areaInterface) - , _rtlManager (_managerSettings, _areaInterface) - , _currentManager (&_defaultManager) - , _managerList {&_defaultManager, &_snakeManager, &_rtlManager} - , _metaDataMap ( - FactMetaData::createMapFromJsonFile( - QStringLiteral(":/json/WimaController.SettingsGroup.json"), this)) - , _enableWimaController (settingsGroup, _metaDataMap[enableWimaControllerName]) - , _overlapWaypoints (settingsGroup, _metaDataMap[overlapWaypointsName]) - , _maxWaypointsPerPhase (settingsGroup, _metaDataMap[maxWaypointsPerPhaseName]) - , _nextPhaseStartWaypointIndex (settingsGroup, _metaDataMap[startWaypointIndexName]) - , _showAllMissionItems (settingsGroup, _metaDataMap[showAllMissionItemsName]) - , _showCurrentMissionItems (settingsGroup, _metaDataMap[showCurrentMissionItemsName]) - , _flightSpeed (settingsGroup, _metaDataMap[flightSpeedName]) - , _arrivalReturnSpeed (settingsGroup, _metaDataMap[arrivalReturnSpeedName]) - , _altitude (settingsGroup, _metaDataMap[altitudeName]) - , _snakeTileWidth (settingsGroup, _metaDataMap[snakeTileWidthName]) - , _snakeTileHeight (settingsGroup, _metaDataMap[snakeTileHeightName]) - , _snakeMinTileArea (settingsGroup, _metaDataMap[snakeMinTileAreaName]) - , _snakeLineDistance (settingsGroup, _metaDataMap[snakeLineDistanceName]) - , _snakeMinTransectLength (settingsGroup, _metaDataMap[snakeMinTransectLengthName]) - , _lowBatteryHandlingTriggered (false) - , _measurementPathLength (-1) - , _snakeCalcInProgress (false) - , _nemoHeartbeat (0 /*status: not connected*/) - , _fallbackStatus (0 /*status: not connected*/) - , _batteryLevelTicker (EVENT_TIMER_INTERVAL, 1000 /*ms*/) - , _snakeTicker (EVENT_TIMER_INTERVAL, 200 /*ms*/) - , _nemoTimeoutTicker (EVENT_TIMER_INTERVAL, 5000 /*ms*/) - , _topicServiceSetupDone (false) -{ - - // Set up facts. - _showAllMissionItems.setRawValue(true); - _showCurrentMissionItems.setRawValue(true); - connect(&_overlapWaypoints, &Fact::rawValueChanged, this, &WimaController::_updateOverlap); - connect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, this, &WimaController::_updateMaxWaypoints); - connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::_setStartIndex); - connect(&_flightSpeed, &Fact::rawValueChanged, this, &WimaController::_updateflightSpeed); - connect(&_arrivalReturnSpeed, &Fact::rawValueChanged, this, &WimaController::_updateArrivalReturnSpeed); - connect(&_altitude, &Fact::rawValueChanged, this, &WimaController::_updateAltitude); - - // Init waypoint managers. - bool value; - size_t overlap = _overlapWaypoints.rawValue().toUInt(&value); - Q_ASSERT(value); - size_t N = _maxWaypointsPerPhase.rawValue().toUInt(&value); - Q_ASSERT(value); - size_t startIdx = _nextPhaseStartWaypointIndex.rawValue().toUInt(&value); - Q_ASSERT(value); - (void)value; - for (auto manager : _managerList){ - manager->setOverlap(overlap); - manager->setN(N); - manager->setStartIndex(startIdx); - } - - // Periodic. - connect(&_eventTimer, &QTimer::timeout, this, &WimaController::_eventTimerHandler); - //_eventTimer.setInterval(EVENT_TIMER_INTERVAL); - _eventTimer.start(EVENT_TIMER_INTERVAL); - - // Snake Worker Thread. - connect(&_snakeDataManager, &SnakeDataManager::finished, this, &WimaController::_snakeStoreWorkerResults); - connect(this, &WimaController::nemoProgressChanged, this, &WimaController::_initStartSnakeWorker); - connect(this, &QObject::destroyed, &this->_snakeDataManager, &SnakeDataManager::quit); - - // Snake. - auto configRosBridge = [this]{ - if ( this->_enableSnake.rawValue().toBool() ){ - this->_snakeDataManager.enableRosBridge(); - } else { - this->_snakeDataManager.disableRosBride(); - } - }; - connect(&_enableSnake, &Fact::rawValueChanged, configRosBridge); - configRosBridge(); - - connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_switchSnakeManager); - _switchSnakeManager(_enableSnake.rawValue()); + : QObject(parent), _joinedArea(), _measurementArea(), _serviceArea(), + _corridor(), _localPlanDataValid(false), + _areaInterface(&_measurementArea, &_serviceArea, &_corridor, + &_joinedArea), + _WMSettings(), _defaultWM(_WMSettings, _areaInterface), + _snakeWM(_WMSettings, _areaInterface), + _rtlWM(_WMSettings, _areaInterface), + _currentWM(&_defaultWM), _WMList{&_defaultWM, &_snakeWM, &_rtlWM}, + _metaDataMap(FactMetaData::createMapFromJsonFile( + QStringLiteral(":/json/WimaController.SettingsGroup.json"), this)), + _enableWimaController(settingsGroup, + _metaDataMap[enableWimaControllerName]), + _overlapWaypoints(settingsGroup, _metaDataMap[overlapWaypointsName]), + _maxWaypointsPerPhase(settingsGroup, + _metaDataMap[maxWaypointsPerPhaseName]), + _nextPhaseStartWaypointIndex(settingsGroup, + _metaDataMap[startWaypointIndexName]), + _showAllMissionItems(settingsGroup, + _metaDataMap[showAllMissionItemsName]), + _showCurrentMissionItems(settingsGroup, + _metaDataMap[showCurrentMissionItemsName]), + _flightSpeed(settingsGroup, _metaDataMap[flightSpeedName]), + _arrivalReturnSpeed(settingsGroup, _metaDataMap[arrivalReturnSpeedName]), + _altitude(settingsGroup, _metaDataMap[altitudeName]), + _snakeTileWidth(settingsGroup, _metaDataMap[snakeTileWidthName]), + _snakeTileHeight(settingsGroup, _metaDataMap[snakeTileHeightName]), + _snakeMinTileArea(settingsGroup, _metaDataMap[snakeMinTileAreaName]), + _snakeLineDistance(settingsGroup, _metaDataMap[snakeLineDistanceName]), + _snakeMinTransectLength(settingsGroup, + _metaDataMap[snakeMinTransectLengthName]), + _lowBatteryHandlingTriggered(false), _measurementPathLength(-1), + _currentDM(&_emptyDM), + _batteryLevelTicker(EVENT_TIMER_INTERVAL, 1000 /*ms*/) { + + // Set up facts. + _showAllMissionItems.setRawValue(true); + _showCurrentMissionItems.setRawValue(true); + connect(&_overlapWaypoints, &Fact::rawValueChanged, this, + &WimaController::_updateOverlap); + connect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, this, + &WimaController::_updateMaxWaypoints); + connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, + &WimaController::_setStartIndex); + connect(&_flightSpeed, &Fact::rawValueChanged, this, + &WimaController::_updateflightSpeed); + connect(&_arrivalReturnSpeed, &Fact::rawValueChanged, this, + &WimaController::_updateArrivalReturnSpeed); + connect(&_altitude, &Fact::rawValueChanged, this, + &WimaController::_updateAltitude); + + // Init waypoint managers. + bool value; + size_t overlap = _overlapWaypoints.rawValue().toUInt(&value); + Q_ASSERT(value); + size_t N = _maxWaypointsPerPhase.rawValue().toUInt(&value); + Q_ASSERT(value); + size_t startIdx = _nextPhaseStartWaypointIndex.rawValue().toUInt(&value); + Q_ASSERT(value); + (void)value; + for (auto manager : _WMList) { + manager->setOverlap(overlap); + manager->setN(N); + manager->setStartIndex(startIdx); + } + + // Periodic. + connect(&_eventTimer, &QTimer::timeout, this, + &WimaController::_eventTimerHandler); + _eventTimer.start(EVENT_TIMER_INTERVAL); + + // Snake Data Manager. + connect(_currentDM, &SnakeDataManager::finished, this, + &WimaController::_DMFinishedHandler); + connect(_currentDM, &SnakeDataManager::nemoProgressChanged, this, + &WimaController::_progressChangedHandler); + connect(_currentDM, &SnakeDataManager::nemoStatusChanged, this, + &WimaController::nemoStatusChanged); + connect(_currentDM, &SnakeDataManager::nemoStatusChanged, this, + &WimaController::nemoStatusStringChanged); + + connect(this, &QObject::destroyed, &this->_snakeDM, &SnakeDataManager::quit); + connect(this, &QObject::destroyed, &this->_emptyDM, &SnakeDataManager::quit); + + connect(&_enableSnake, &Fact::rawValueChanged, this, + &WimaController::_enableSnakeChangedHandler); + _enableSnakeChangedHandler(); + + // Snake Waypoint Manager. + connect(&_enableSnake, &Fact::rawValueChanged, this, + &WimaController::_switchToSnakeWaypointManager); + _switchToSnakeWaypointManager(_enableSnake.rawValue()); } PlanMasterController *WimaController::masterController() { - return _masterController; + return _masterController; } MissionController *WimaController::missionController() { - return _missionController; + return _missionController; } -QmlObjectListModel *WimaController::visualItems() { - return &_areas; -} +QmlObjectListModel *WimaController::visualItems() { return &_areas; } QmlObjectListModel *WimaController::missionItems() { - return const_cast(&_currentManager->missionItems()); + return const_cast(&_currentWM->missionItems()); } QmlObjectListModel *WimaController::currentMissionItems() { - return const_cast(&_currentManager->currentMissionItems()); + return const_cast(&_currentWM->currentMissionItems()); } -QVariantList WimaController::waypointPath() -{ - return const_cast(_currentManager->waypointsVariant()); +QVariantList WimaController::waypointPath() { + return const_cast(_currentWM->waypointsVariant()); } -QVariantList WimaController::currentWaypointPath() -{ - return const_cast(_currentManager->currentWaypointsVariant()); +QVariantList WimaController::currentWaypointPath() { + return const_cast(_currentWM->currentWaypointsVariant()); } -Fact *WimaController::enableWimaController() { - return &_enableWimaController; -} +Fact *WimaController::enableWimaController() { return &_enableWimaController; } -Fact *WimaController::overlapWaypoints() { - return &_overlapWaypoints; -} +Fact *WimaController::overlapWaypoints() { return &_overlapWaypoints; } -Fact *WimaController::maxWaypointsPerPhase() { - return &_maxWaypointsPerPhase; -} +Fact *WimaController::maxWaypointsPerPhase() { return &_maxWaypointsPerPhase; } Fact *WimaController::startWaypointIndex() { - return &_nextPhaseStartWaypointIndex; + return &_nextPhaseStartWaypointIndex; } -Fact *WimaController::showAllMissionItems() { - return &_showAllMissionItems; -} +Fact *WimaController::showAllMissionItems() { return &_showAllMissionItems; } Fact *WimaController::showCurrentMissionItems() { - return &_showCurrentMissionItems; + return &_showCurrentMissionItems; } -Fact *WimaController::flightSpeed() { - return &_flightSpeed; -} +Fact *WimaController::flightSpeed() { return &_flightSpeed; } -Fact *WimaController::arrivalReturnSpeed() { - return &_arrivalReturnSpeed; -} +Fact *WimaController::arrivalReturnSpeed() { return &_arrivalReturnSpeed; } -Fact *WimaController::altitude() { - return &_altitude; -} +Fact *WimaController::altitude() { return &_altitude; } -QmlObjectListModel *WimaController::snakeTiles() -{ - static QmlObjectListModel list; - qWarning() << "using snake tile dummy"; - return &list; +QmlObjectListModel *WimaController::snakeTiles() { + return const_cast(this->_snakeDM.tiles()); } -QVariantList WimaController::snakeTileCenterPoints() -{ - qWarning() << "using snakeTileCenterPoints dummy"; - return QVariantList(); +QVariantList WimaController::snakeTileCenterPoints() { + return _currentDM->tileCenterPoints(); } -QVector WimaController::nemoProgress() -{ - qWarning() << "using nemoProgress dummy"; - return QVector(); +QVector WimaController::nemoProgress() { + return _currentDM->nemoProgress().progress(); } -double WimaController::phaseDistance() const -{ - return 0.0; +double WimaController::phaseDistance() const { + qWarning() << "using phaseDistance dummy"; + return 0.0; } -double WimaController::phaseDuration() const -{ - return 0.0; +double WimaController::phaseDuration() const { + qWarning() << "using phaseDuration dummy"; + return 0.0; } -int WimaController::nemoStatus() const -{ - qWarning() << "using nemoStatus dummy"; - return 0; -} +int WimaController::nemoStatus() const { return _currentDM->nemoStatus(); } -QString WimaController::nemoStatusString() const -{ - return _nemoStatusMap.at(nemoStatus()); +QString WimaController::nemoStatusString() const { + return _nemoStatusMap.at(nemoStatus()); } -bool WimaController::snakeCalcInProgress() const -{ - qWarning() << "using snakeCalcInProgress dummy"; - return false; +bool WimaController::snakeCalcInProgress() const { + return _currentDM->calcInProgress(); } -void WimaController::setMasterController(PlanMasterController *masterC) -{ - _masterController = masterC; - _managerSettings.setMasterController(masterC); - emit masterControllerChanged(); +void WimaController::setMasterController(PlanMasterController *masterC) { + _masterController = masterC; + _WMSettings.setMasterController(masterC); + emit masterControllerChanged(); } -void WimaController::setMissionController(MissionController *missionC) -{ - _missionController = missionC; - _managerSettings.setMissionController(missionC); - emit missionControllerChanged(); +void WimaController::setMissionController(MissionController *missionC) { + _missionController = missionC; + _WMSettings.setMissionController(missionC); + emit missionControllerChanged(); } -void WimaController::nextPhase() -{ - _calcNextPhase(); -} +void WimaController::nextPhase() { _calcNextPhase(); } -void WimaController::previousPhase() -{ - if ( !_currentManager->previous() ) { - Q_ASSERT(false); - } +void WimaController::previousPhase() { + if (!_currentWM->previous()) { + Q_ASSERT(false); + } - emit missionItemsChanged(); - emit currentMissionItemsChanged(); - emit currentWaypointPathChanged(); - emit waypointPathChanged(); + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit currentWaypointPathChanged(); + emit waypointPathChanged(); } -void WimaController::resetPhase() -{ - if ( !_currentManager->reset() ) { - Q_ASSERT(false); - } +void WimaController::resetPhase() { + if (!_currentWM->reset()) { + Q_ASSERT(false); + } - emit missionItemsChanged(); - emit currentMissionItemsChanged(); - emit currentWaypointPathChanged(); - emit waypointPathChanged(); + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit currentWaypointPathChanged(); + emit waypointPathChanged(); } -void WimaController::requestSmartRTL() -{ - QString errorString("Smart RTL requested. "); - if ( !_checkSmartRTLPreCondition(errorString) ){ - qgcApp()->showMessage(errorString); - return; - } - emit smartRTLRequestConfirm(); +void WimaController::requestSmartRTL() { + QString errorString("Smart RTL requested. "); + if (!_checkSmartRTLPreCondition(errorString)) { + qgcApp()->showMessage(errorString); + return; + } + emit smartRTLRequestConfirm(); } -bool WimaController::upload() -{ - auto ¤tMissionItems = _defaultManager.currentMissionItems(); - if ( !_serviceArea.containsCoordinate(_masterController->managerVehicle()->coordinate()) - && currentMissionItems.count() > 0) { - emit forceUploadConfirm(); - return false; - } +bool WimaController::upload() { + auto ¤tMissionItems = _defaultWM.currentMissionItems(); + if (!_serviceArea.containsCoordinate( + _masterController->managerVehicle()->coordinate()) && + currentMissionItems.count() > 0) { + emit forceUploadConfirm(); + return false; + } - return forceUpload(); + return forceUpload(); } -bool WimaController::forceUpload() -{ - auto ¤tMissionItems = _defaultManager.currentMissionItems(); - if (currentMissionItems.count() < 1) - return false; +bool WimaController::forceUpload() { + auto ¤tMissionItems = _defaultWM.currentMissionItems(); + if (currentMissionItems.count() < 1) + return false; - _missionController->removeAll(); - // Set homeposition of settingsItem. - QmlObjectListModel* visuals = _missionController->visualItems(); - MissionSettingsItem* settingsItem = visuals->value(0); - if (settingsItem == nullptr) { - Q_ASSERT(false); - qWarning("WimaController::updateCurrentMissionItems(): nullptr"); - return false; - } - settingsItem->setCoordinate(_managerSettings.homePosition()); + _missionController->removeAll(); + // Set homeposition of settingsItem. + QmlObjectListModel *visuals = _missionController->visualItems(); + MissionSettingsItem *settingsItem = visuals->value(0); + if (settingsItem == nullptr) { + Q_ASSERT(false); + qWarning("WimaController::updateCurrentMissionItems(): nullptr"); + return false; + } + settingsItem->setCoordinate(_WMSettings.homePosition()); - // Copy mission items to _missionController. - for (int i = 1; i < currentMissionItems.count(); i++){ - auto *item = currentMissionItems.value(i); - _missionController->insertSimpleMissionItem(*item, visuals->count()); - } + // Copy mission items to _missionController. + for (int i = 1; i < currentMissionItems.count(); i++) { + auto *item = currentMissionItems.value(i); + _missionController->insertSimpleMissionItem(*item, visuals->count()); + } - _masterController->sendToVehicle(); + _masterController->sendToVehicle(); - return true; + return true; } -void WimaController::removeFromVehicle() -{ - _masterController->removeAllFromVehicle(); - _missionController->removeAll(); +void WimaController::removeFromVehicle() { + _masterController->removeAllFromVehicle(); + _missionController->removeAll(); } -void WimaController::executeSmartRTL() -{ - forceUpload(); - masterController()->managerVehicle()->startMission(); +void WimaController::executeSmartRTL() { + forceUpload(); + masterController()->managerVehicle()->startMission(); } -void WimaController::initSmartRTL() -{ - _initSmartRTL(); -} +void WimaController::initSmartRTL() { _initSmartRTL(); } -void WimaController::removeVehicleTrajectoryHistory() -{ - Vehicle *managerVehicle = masterController()->managerVehicle(); - managerVehicle->trajectoryPoints()->clear(); +void WimaController::removeVehicleTrajectoryHistory() { + Vehicle *managerVehicle = masterController()->managerVehicle(); + managerVehicle->trajectoryPoints()->clear(); } bool WimaController::_calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, - QVector &path) -{ - using namespace GeoUtilities; - using namespace PolygonCalculus; - QPolygonF polygon2D; - toCartesianList(_joinedArea.coordinateList(), /*origin*/ start, polygon2D); - QPointF start2D(0,0); - QPointF end2D; - toCartesian(destination, start, end2D); - QVector path2D; - - bool retVal = PolygonCalculus::shortestPath(polygon2D, start2D, end2D, path2D); - toGeoList(path2D, /*origin*/ start, path); - - return retVal; -} - -bool WimaController::setWimaPlanData(const WimaPlanData &planData) -{ - // reset visual items - _areas.clear(); - _defaultManager.clear(); - _snakeManager.clear(); - _snakeTiles.polygons().clear(); - _snakeTilesLocal.polygons().clear(); - _snakeTileCenterPoints.clear(); - - emit visualItemsChanged(); - emit missionItemsChanged(); - emit currentMissionItemsChanged(); - emit waypointPathChanged(); - emit currentWaypointPathChanged(); - emit snakeTilesChanged(); - emit snakeTileCenterPointsChanged(); - - _localPlanDataValid = false; - - // extract list with WimaAreas - QList areaList = planData.areaList(); + QVector &path) { + using namespace GeoUtilities; + using namespace PolygonCalculus; + QPolygonF polygon2D; + toCartesianList(_joinedArea.coordinateList(), /*origin*/ start, polygon2D); + QPointF start2D(0, 0); + QPointF end2D; + toCartesian(destination, start, end2D); + QVector path2D; - int areaCounter = 0; - const int numAreas = 4; // extract only numAreas Areas, if there are more they are invalid and ignored - for (int i = 0; i < areaList.size(); i++) { - const WimaAreaData *areaData = areaList[i]; + bool retVal = + PolygonCalculus::shortestPath(polygon2D, start2D, end2D, path2D); + toGeoList(path2D, /*origin*/ start, path); - if (areaData->type() == WimaServiceAreaData::typeString) { // is it a service area? - _serviceArea = *qobject_cast(areaData); - areaCounter++; - _areas.append(&_serviceArea); - - continue; - } - - if (areaData->type() == WimaMeasurementAreaData::typeString) { // is it a measurement area? - _measurementArea = *qobject_cast(areaData); - areaCounter++; - _areas.append(&_measurementArea); + return retVal; +} - continue; - } +bool WimaController::setWimaPlanData(const WimaPlanData &planData) { + // reset visual items + _areas.clear(); + _defaultWM.clear(); + _snakeWM.clear(); - if (areaData->type() == WimaCorridorData::typeString) { // is it a corridor? - _corridor = *qobject_cast(areaData); - areaCounter++; - //_visualItems.append(&_corridor); // not needed + emit visualItemsChanged(); + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit waypointPathChanged(); + emit currentWaypointPathChanged(); - continue; - } + _localPlanDataValid = false; - if (areaData->type() == WimaJoinedAreaData::typeString) { // is it a corridor? - _joinedArea = *qobject_cast(areaData); - areaCounter++; - _areas.append(&_joinedArea); + // extract list with WimaAreas + QList areaList = planData.areaList(); - continue; - } + int areaCounter = 0; + const int numAreas = 4; // extract only numAreas Areas, if there are more they + // are invalid and ignored + for (int i = 0; i < areaList.size(); i++) { + const WimaAreaData *areaData = areaList[i]; - if (areaCounter >= numAreas) - break; - } + if (areaData->type() == + WimaServiceAreaData::typeString) { // is it a service area? + _serviceArea = *qobject_cast(areaData); + areaCounter++; + _areas.append(&_serviceArea); - if (areaCounter != numAreas) { - Q_ASSERT(false); - return false; + continue; } - emit visualItemsChanged(); - - // extract mission items - QList tempMissionItems = planData.missionItems(); - if (tempMissionItems.size() < 1) { - qWarning("WimaController: Mission items from WimaPlaner empty!"); - return false; - } + if (areaData->type() == + WimaMeasurementAreaData::typeString) { // is it a measurement area? + _measurementArea = + *qobject_cast(areaData); + areaCounter++; + _areas.append(&_measurementArea); - for (auto item : tempMissionItems) { - _defaultManager.push_back(item.coordinate()); + continue; } - _managerSettings.setHomePosition( QGeoCoordinate(_serviceArea.center().latitude(), - _serviceArea.center().longitude(), - 0) ); + if (areaData->type() == WimaCorridorData::typeString) { // is it a corridor? + _corridor = *qobject_cast(areaData); + areaCounter++; + //_visualItems.append(&_corridor); // not needed - if( !_defaultManager.reset() ){ - Q_ASSERT(false); - return false; + continue; } - emit missionItemsChanged(); - emit currentMissionItemsChanged(); - emit waypointPathChanged(); - emit currentWaypointPathChanged(); - - - - // Initialize _scenario. - Area mArea; - for (auto variant : _measurementArea.path()){ - QGeoCoordinate c{variant.value()}; - mArea.geoPolygon.push_back(GeoPoint2D{c.latitude(), c.longitude()}); - } - mArea.type = AreaType::MeasurementArea; + if (areaData->type() == + WimaJoinedAreaData::typeString) { // is it a corridor? + _joinedArea = *qobject_cast(areaData); + areaCounter++; + _areas.append(&_joinedArea); - Area sArea; - for (auto variant : _serviceArea.path()){ - QGeoCoordinate c{variant.value()}; - sArea.geoPolygon.push_back(GeoPoint2D{c.latitude(), c.longitude()}); + continue; } - sArea.type = AreaType::ServiceArea; - Area corridor; - for (auto variant : _corridor.path()){ - QGeoCoordinate c{variant.value()}; - corridor.geoPolygon.push_back(GeoPoint2D{c.latitude(), c.longitude()}); - } - corridor.type = AreaType::Corridor; - - _scenario.addArea(mArea); - _scenario.addArea(sArea); - _scenario.addArea(corridor); - - // Check if scenario is defined. - if ( !_scenario.defined(_snakeTileWidth.rawValue().toUInt(), - _snakeTileHeight.rawValue().toUInt(), - _snakeMinTileArea.rawValue().toUInt()) ) { - Q_ASSERT(false); - return false; - } - - { - // Get tiles and origin. - auto origin = _scenario.getOrigin(); - _snakeOrigin.setLatitude(origin[0]); - _snakeOrigin.setLongitude(origin[1]); - _snakeOrigin.setAltitude(origin[2]); - const auto &tiles = _scenario.getTiles(); - const auto &cps = _scenario.getTileCenterPoints(); - for ( unsigned int i=0; i < tiles.size(); ++i ) { - const auto &tile = tiles[i]; - SnakeTile Tile; - for ( const auto &vertex : tile) { - QGeoCoordinate QVertex(vertex[0], vertex[1], vertex[2]); - Tile.append(QVertex); - } - const auto ¢erPoint = cps[i]; - QGeoCoordinate QCenterPoint(centerPoint[0], centerPoint[1], centerPoint[2]); - Tile.setCenter(QCenterPoint); - _snakeTiles.polygons().append(Tile); - _snakeTileCenterPoints.append(QVariant::fromValue(QCenterPoint)); - } - } - - { - // Get local tiles. - const auto &tiles = _scenario.getTilesENU(); - for ( unsigned int i=0; i < tiles.size(); ++i ) { - const auto &tile = tiles[i]; - Polygon2D Tile; - for ( const auto &vertex : tile.outer()) { - QPointF QVertex(vertex.get<0>(), vertex.get<1>()); - Tile.path().append(QVertex); - } - _snakeTilesLocal.polygons().append(Tile); - } - } - emit snakeTilesChanged(); - emit snakeTileCenterPointsChanged(); - - if ( _enableSnake.rawValue().toBool() - && _pRosBridge->isRunning() - && _pRosBridge->connected() - && _topicServiceSetupDone - && _snakeTilesLocal.polygons().size() > 0 - ) - { - using namespace ros_bridge::messages; - // Publish snake origin. - JsonDocUPtr jOrigin(std::make_unique(rapidjson::kObjectType)); - bool ret = geographic_msgs::geo_point::toJson( - _snakeOrigin, *jOrigin, jOrigin->GetAllocator()); - Q_ASSERT(ret); - _pRosBridge->publish(std::move(jOrigin), "/snake/origin"); - // Publish snake tiles. - JsonDocUPtr jSnakeTiles(std::make_unique(rapidjson::kObjectType)); - ret = jsk_recognition_msgs::polygon_array::toJson( - _snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator()); - Q_ASSERT(ret); - _pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles"); - - } - - - _localPlanDataValid = true; - return true; -} - -WimaController *WimaController::thisPointer() -{ - return this; -} - -bool WimaController::_calcNextPhase() -{ - if ( !_currentManager->next() ) { - Q_ASSERT(false); - return false; - } - - emit missionItemsChanged(); - emit currentMissionItemsChanged(); - emit currentWaypointPathChanged(); - emit waypointPathChanged(); - - return true; -} + if (areaCounter >= numAreas) + break; + } -bool WimaController::_setStartIndex() -{ - bool value; - _currentManager->setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt(&value)-1); - Q_ASSERT(value); - (void)value; + if (areaCounter != numAreas) { + Q_ASSERT(false); + return false; + } - if ( !_currentManager->update() ) { - Q_ASSERT(false); - return false; - } + emit visualItemsChanged(); - emit missionItemsChanged(); - emit currentMissionItemsChanged(); - emit currentWaypointPathChanged(); - emit waypointPathChanged(); + // extract mission items + QList tempMissionItems = planData.missionItems(); + if (tempMissionItems.size() < 1) { + qWarning("WimaController: Mission items from WimaPlaner empty!"); + return false; + } - return true; -} + for (auto item : tempMissionItems) { + _defaultWM.push_back(item.coordinate()); + } -void WimaController::_recalcCurrentPhase() -{ - if ( !_currentManager->update() ) { - Q_ASSERT(false); - } + _WMSettings.setHomePosition(QGeoCoordinate( + _serviceArea.center().latitude(), _serviceArea.center().longitude(), 0)); - emit missionItemsChanged(); - emit currentMissionItemsChanged(); - emit currentWaypointPathChanged(); - emit waypointPathChanged(); -} + if (!_defaultWM.reset()) { + Q_ASSERT(false); + return false; + } -void WimaController::_updateOverlap() -{ - bool value; - _currentManager->setOverlap(_overlapWaypoints.rawValue().toUInt(&value)); - Q_ASSERT(value); - (void)value; + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit waypointPathChanged(); + emit currentWaypointPathChanged(); - if ( !_currentManager->update() ) { - assert(false); - } + // Update Snake Data Manager + _snakeDM.setMeasurementArea(_measurementArea.coordinateList()); + _snakeDM.setServiceArea(_serviceArea.coordinateList()); + _snakeDM.setCorridor(_corridor.coordinateList()); + _currentDM->start(); - emit missionItemsChanged(); - emit currentMissionItemsChanged(); - emit currentWaypointPathChanged(); - emit waypointPathChanged(); + _localPlanDataValid = true; + return true; } -void WimaController::_updateMaxWaypoints() -{ - bool value; - _currentManager->setN(_maxWaypointsPerPhase.rawValue().toUInt(&value)); - Q_ASSERT(value); - (void)value; - - if ( !_currentManager->update() ) { - Q_ASSERT(false); - } - - emit missionItemsChanged(); - emit currentMissionItemsChanged(); - emit currentWaypointPathChanged(); - emit waypointPathChanged(); -} +WimaController *WimaController::thisPointer() { return this; } -void WimaController::_updateflightSpeed() -{ - bool value; - _managerSettings.setFlightSpeed(_flightSpeed.rawValue().toDouble(&value)); - Q_ASSERT(value); - (void)value; +bool WimaController::_calcNextPhase() { + if (!_currentWM->next()) { + Q_ASSERT(false); + return false; + } - if ( !_currentManager->update() ) { - Q_ASSERT(false); - } + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit currentWaypointPathChanged(); + emit waypointPathChanged(); - emit missionItemsChanged(); - emit currentMissionItemsChanged(); - emit currentWaypointPathChanged(); - emit waypointPathChanged(); + return true; } -void WimaController::_updateArrivalReturnSpeed() -{ - bool value; - _managerSettings.setArrivalReturnSpeed(_arrivalReturnSpeed.rawValue().toDouble(&value)); - Q_ASSERT(value); - (void)value; +bool WimaController::_setStartIndex() { + bool value; + _currentWM->setStartIndex( + _nextPhaseStartWaypointIndex.rawValue().toUInt(&value) - 1); + Q_ASSERT(value); + (void)value; - if ( !_currentManager->update() ) { - Q_ASSERT(false); - } - - emit missionItemsChanged(); - emit currentMissionItemsChanged(); - emit currentWaypointPathChanged(); - emit waypointPathChanged(); -} - -void WimaController::_updateAltitude() -{ - bool value; - _managerSettings.setAltitude(_altitude.rawValue().toDouble(&value)); - Q_ASSERT(value); - (void)value; + if (!_currentWM->update()) { + Q_ASSERT(false); + return false; + } - if ( !_currentManager->update() ) { - Q_ASSERT(false); - } + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit currentWaypointPathChanged(); + emit waypointPathChanged(); - emit missionItemsChanged(); - emit currentMissionItemsChanged(); - emit currentWaypointPathChanged(); - emit waypointPathChanged(); + return true; } -void WimaController::_checkBatteryLevel() -{ - Vehicle *managerVehicle = masterController()->managerVehicle(); - WimaSettings* wimaSettings = qgcApp()->toolbox()->settingsManager()->wimaSettings(); - int batteryThreshold = wimaSettings->lowBatteryThreshold()->rawValue().toInt(); - bool enabled = _enableWimaController.rawValue().toBool(); - unsigned int minTime = wimaSettings->minimalRemainingMissionTime()->rawValue().toUInt(); - - if (managerVehicle != nullptr && enabled == true) { - Fact *battery1percentRemaining = managerVehicle->battery1FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName); - Fact *battery2percentRemaining = managerVehicle->battery2FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName); +void WimaController::_recalcCurrentPhase() { + if (!_currentWM->update()) { + Q_ASSERT(false); + } - - if (battery1percentRemaining->rawValue().toDouble() < batteryThreshold - && battery2percentRemaining->rawValue().toDouble() < batteryThreshold) { - if (!_lowBatteryHandlingTriggered) { - _lowBatteryHandlingTriggered = true; - if ( !(_missionController->remainingTime() <= minTime) ) { - requestSmartRTL(); - } - } - } - else { - _lowBatteryHandlingTriggered = false; + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit currentWaypointPathChanged(); + emit waypointPathChanged(); +} + +void WimaController::_updateOverlap() { + bool value; + _currentWM->setOverlap(_overlapWaypoints.rawValue().toUInt(&value)); + Q_ASSERT(value); + (void)value; + + if (!_currentWM->update()) { + assert(false); + } + + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit currentWaypointPathChanged(); + emit waypointPathChanged(); +} + +void WimaController::_updateMaxWaypoints() { + bool value; + _currentWM->setN(_maxWaypointsPerPhase.rawValue().toUInt(&value)); + Q_ASSERT(value); + (void)value; + + if (!_currentWM->update()) { + Q_ASSERT(false); + } + + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit currentWaypointPathChanged(); + emit waypointPathChanged(); +} + +void WimaController::_updateflightSpeed() { + bool value; + _WMSettings.setFlightSpeed(_flightSpeed.rawValue().toDouble(&value)); + Q_ASSERT(value); + (void)value; + + if (!_currentWM->update()) { + Q_ASSERT(false); + } + + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit currentWaypointPathChanged(); + emit waypointPathChanged(); +} + +void WimaController::_updateArrivalReturnSpeed() { + bool value; + _WMSettings.setArrivalReturnSpeed( + _arrivalReturnSpeed.rawValue().toDouble(&value)); + Q_ASSERT(value); + (void)value; + + if (!_currentWM->update()) { + Q_ASSERT(false); + } + + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit currentWaypointPathChanged(); + emit waypointPathChanged(); +} + +void WimaController::_updateAltitude() { + bool value; + _WMSettings.setAltitude(_altitude.rawValue().toDouble(&value)); + Q_ASSERT(value); + (void)value; + + if (!_currentWM->update()) { + Q_ASSERT(false); + } + + emit missionItemsChanged(); + emit currentMissionItemsChanged(); + emit currentWaypointPathChanged(); + emit waypointPathChanged(); +} + +void WimaController::_checkBatteryLevel() { + Vehicle *managerVehicle = masterController()->managerVehicle(); + WimaSettings *wimaSettings = + qgcApp()->toolbox()->settingsManager()->wimaSettings(); + int batteryThreshold = + wimaSettings->lowBatteryThreshold()->rawValue().toInt(); + bool enabled = _enableWimaController.rawValue().toBool(); + unsigned int minTime = + wimaSettings->minimalRemainingMissionTime()->rawValue().toUInt(); + + if (managerVehicle != nullptr && enabled == true) { + Fact *battery1percentRemaining = + managerVehicle->battery1FactGroup()->getFact( + VehicleBatteryFactGroup::_percentRemainingFactName); + Fact *battery2percentRemaining = + managerVehicle->battery2FactGroup()->getFact( + VehicleBatteryFactGroup::_percentRemainingFactName); + + if (battery1percentRemaining->rawValue().toDouble() < batteryThreshold && + battery2percentRemaining->rawValue().toDouble() < batteryThreshold) { + if (!_lowBatteryHandlingTriggered) { + _lowBatteryHandlingTriggered = true; + if (!(_missionController->remainingTime() <= minTime)) { + requestSmartRTL(); } - + } + } else { + _lowBatteryHandlingTriggered = false; } + } } -void WimaController::_eventTimerHandler() -{ - // Battery level check necessary? - Fact *enableLowBatteryHandling = qgcApp()->toolbox()->settingsManager()->wimaSettings()->enableLowBatteryHandling(); - if ( enableLowBatteryHandling->rawValue().toBool() && _batteryLevelTicker.ready() ) - _checkBatteryLevel(); - - // Snake flight plan update necessary? -// if ( snakeEventLoopTicker.ready() ) { -// if ( _enableSnake.rawValue().toBool() && _localPlanDataValid && !_snakeCalcInProgress && _scenarioDefinedBool) { -// } -// } - - if ( _nemoTimeoutTicker.ready() && _enableSnake.rawValue().toBool() ) { - this->_nemoHeartbeat.setStatus(_fallbackStatus); - emit WimaController::nemoStatusChanged(); - emit WimaController::nemoStatusStringChanged(); - } +void WimaController::_eventTimerHandler() { + // Battery level check necessary? + Fact *enableLowBatteryHandling = qgcApp() + ->toolbox() + ->settingsManager() + ->wimaSettings() + ->enableLowBatteryHandling(); + if (enableLowBatteryHandling->rawValue().toBool() && + _batteryLevelTicker.ready()) + _checkBatteryLevel(); } -void WimaController::_smartRTLCleanUp(bool flying) -{ - if ( !flying) { // vehicle has landed - _switchWaypointManager(_defaultManager); - _missionController->removeAllFromVehicle(); - _missionController->removeAll(); - disconnect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp); - } -} - -void WimaController::_setPhaseDistance(double distance) -{ - (void)distance; -// if (!qFuzzyCompare(distance, _phaseDistance)) { -// _phaseDistance = distance; - -// emit phaseDistanceChanged(); -// } +void WimaController::_smartRTLCleanUp(bool flying) { + if (!flying) { // vehicle has landed + _switchWaypointManager(_defaultWM); + _missionController->removeAllFromVehicle(); + _missionController->removeAll(); + disconnect(masterController()->managerVehicle(), &Vehicle::flyingChanged, + this, &WimaController::_smartRTLCleanUp); + } } -void WimaController::_setPhaseDuration(double duration) -{ - (void)duration; -// if (!qFuzzyCompare(duration, _phaseDuration)) { -// _phaseDuration = duration; +void WimaController::_setPhaseDistance(double distance) { + (void)distance; + // if (!qFuzzyCompare(distance, _phaseDistance)) { + // _phaseDistance = distance; -// emit phaseDurationChanged(); -// } + // emit phaseDistanceChanged(); + // } } -bool WimaController::_checkSmartRTLPreCondition(QString &errorString) -{ - if (!_localPlanDataValid) { - errorString.append(tr("No WiMA data available. Please define at least a measurement and a service area.")); - return false; - } +void WimaController::_setPhaseDuration(double duration) { + (void)duration; + // if (!qFuzzyCompare(duration, _phaseDuration)) { + // _phaseDuration = duration; - return _rtlManager.checkPrecondition(errorString); + // emit phaseDurationChanged(); + // } } -void WimaController::_switchWaypointManager(WaypointManager::ManagerBase &manager) -{ - if (_currentManager != &manager) { - _currentManager = &manager; - - disconnect(&_overlapWaypoints, &Fact::rawValueChanged, - this, &WimaController::_updateOverlap); - disconnect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, - this, &WimaController::_updateMaxWaypoints); - disconnect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, - this, &WimaController::_setStartIndex); - - _maxWaypointsPerPhase.setRawValue(_currentManager->N()); - _overlapWaypoints.setRawValue(_currentManager->overlap()); - _nextPhaseStartWaypointIndex.setRawValue(_currentManager->startIndex()); - - connect(&_overlapWaypoints, &Fact::rawValueChanged, - this, &WimaController::_updateOverlap); - connect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, - this, &WimaController::_updateMaxWaypoints); - connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, - this, &WimaController::_setStartIndex); - - emit missionItemsChanged(); - emit currentMissionItemsChanged(); - emit waypointPathChanged(); - emit currentWaypointPathChanged(); - - qWarning() << "WimaController::_switchWaypointManager: statistics update missing."; - } -} +bool WimaController::_checkSmartRTLPreCondition(QString &errorString) { + if (!_localPlanDataValid) { + errorString.append(tr("No WiMA data available. Please define at least a " + "measurement and a service area.")); + return false; + } -void WimaController::_initSmartRTL() -{ - QString errorString; - static int attemptCounter = 0; - attemptCounter++; - - if ( _checkSmartRTLPreCondition(errorString) ) { - _masterController->managerVehicle()->pauseVehicle(); - connect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp); - if ( _rtlManager.update() ) { // Calculate return path. - _switchWaypointManager(_rtlManager); - attemptCounter = 0; - emit smartRTLPathConfirm(); - return; - } - } else if (attemptCounter > SMART_RTL_MAX_ATTEMPTS) { - errorString.append(tr("Smart RTL: No success after maximum number of attempts.")); - qgcApp()->showMessage(errorString); - attemptCounter = 0; - } else { - _smartRTLTimer.singleShot(SMART_RTL_ATTEMPT_INTERVAL, this, &WimaController::_initSmartRTL); - } + return _rtlWM.checkPrecondition(errorString); } -void WimaController::_setSnakeCalcInProgress(bool inProgress) -{ - if (_snakeCalcInProgress != inProgress){ - _snakeCalcInProgress = inProgress; - emit snakeCalcInProgressChanged(); - } -} +void WimaController::_switchWaypointManager( + WaypointManager::ManagerBase &manager) { + if (_currentWM != &manager) { + _currentWM = &manager; -void WimaController::_snakeStoreWorkerResults() -{ - _setSnakeCalcInProgress(false); - auto start = std::chrono::high_resolution_clock::now(); - _snakeManager.clear(); + disconnect(&_overlapWaypoints, &Fact::rawValueChanged, this, + &WimaController::_updateOverlap); + disconnect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, this, + &WimaController::_updateMaxWaypoints); + disconnect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, + &WimaController::_setStartIndex); - const auto &r = _snakeDataManager.getResult(); - if (!r.success) { - //qgcApp()->showMessage(r.errorMessage); - return; - } - - // create Mission items from r.waypoints - long n = r.waypoints.size() - r.returnPathIdx.size() - r.arrivalPathIdx.size() + 2; - if (n < 1) { - return; - } - - // Create QVector containing all waypoints; - unsigned long startIdx = r.arrivalPathIdx.last(); - unsigned long endIdx = r.returnPathIdx.first(); - for (unsigned long i = startIdx; i <= endIdx; ++i) { - _snakeManager.push_back(r.waypoints[int(i)].value()); - } + _maxWaypointsPerPhase.setRawValue(_currentWM->N()); + _overlapWaypoints.setRawValue(_currentWM->overlap()); + _nextPhaseStartWaypointIndex.setRawValue(_currentWM->startIndex()); - _snakeManager.update(); + connect(&_overlapWaypoints, &Fact::rawValueChanged, this, + &WimaController::_updateOverlap); + connect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, this, + &WimaController::_updateMaxWaypoints); + connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, + &WimaController::_setStartIndex); emit missionItemsChanged(); emit currentMissionItemsChanged(); - emit currentWaypointPathChanged(); emit waypointPathChanged(); + emit currentWaypointPathChanged(); - auto end = std::chrono::high_resolution_clock::now(); - double duration = std::chrono::duration_cast(end-start).count(); - qWarning() << "WimaController::_snakeStoreWorkerResults execution time: " << duration << " ms."; + qWarning() + << "WimaController::_switchWaypointManager: statistics update missing."; + } +} + +void WimaController::_initSmartRTL() { + QString errorString; + static int attemptCounter = 0; + attemptCounter++; + + if (_checkSmartRTLPreCondition(errorString)) { + _masterController->managerVehicle()->pauseVehicle(); + connect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, + &WimaController::_smartRTLCleanUp); + if (_rtlWM.update()) { // Calculate return path. + _switchWaypointManager(_rtlWM); + attemptCounter = 0; + emit smartRTLPathConfirm(); + return; + } + } else if (attemptCounter > SMART_RTL_MAX_ATTEMPTS) { + errorString.append( + tr("Smart RTL: No success after maximum number of attempts.")); + qgcApp()->showMessage(errorString); + attemptCounter = 0; + } else { + _smartRTLTimer.singleShot(SMART_RTL_ATTEMPT_INTERVAL, this, + &WimaController::_initSmartRTL); + } +} + +void WimaController::_DMFinishedHandler() { + if (!_snakeDM.success()) { + // qgcApp()->showMessage(r.errorMessage); + return; + } + + // Copy waypoints to waypoint manager. + _snakeWM.clear(); + auto waypoints = _snakeDM.waypoints(); + if (waypoints.size() < 1) { + return; + } + for (auto &vertex : waypoints) { + _snakeWM.push_back(vertex); + } + + // Do update. + auto fut = QtConcurrent::run([this] { + this->_snakeWM.update(); // this can take a while (ca. 200ms) + + emit this->missionItemsChanged(); + emit this->currentMissionItemsChanged(); + emit this->currentWaypointPathChanged(); + emit this->waypointPathChanged(); + }); + (void)fut; +} + +void WimaController::_switchToSnakeWaypointManager(QVariant variant) { + if (variant.value()) { + _switchWaypointManager(_snakeWM); + } else { + _switchWaypointManager(_defaultWM); + } +} + +void WimaController::_switchDataManager(SnakeDataManager &dataManager) { + if (_currentDM != &dataManager) { + + disconnect(_currentDM, &SnakeDataManager::finished, this, + &WimaController::_DMFinishedHandler); + disconnect(_currentDM, &SnakeDataManager::nemoProgressChanged, this, + &WimaController::_progressChangedHandler); + disconnect(_currentDM, &SnakeDataManager::nemoStatusChanged, this, + &WimaController::nemoStatusChanged); + disconnect(_currentDM, &SnakeDataManager::nemoStatusChanged, this, + &WimaController::nemoStatusStringChanged); + + _currentDM = &dataManager; + + connect(_currentDM, &SnakeDataManager::finished, this, + &WimaController::_DMFinishedHandler); + connect(_currentDM, &SnakeDataManager::nemoProgressChanged, this, + &WimaController::_progressChangedHandler); + connect(_currentDM, &SnakeDataManager::nemoStatusChanged, this, + &WimaController::nemoStatusChanged); + connect(_currentDM, &SnakeDataManager::nemoStatusChanged, this, + &WimaController::nemoStatusStringChanged); + + emit snakeConnectionStatusChanged(); + emit snakeCalcInProgressChanged(); + emit snakeTilesChanged(); + emit snakeTileCenterPointsChanged(); + emit nemoProgressChanged(); + emit nemoStatusChanged(); + emit nemoStatusStringChanged(); + } } -void WimaController::_initStartSnakeWorker() -{ - if ( !_enableSnake.rawValue().toBool() ) - return; - - // Stop worker thread if running. - if ( _snakeDataManager.isRunning() ) { - _snakeDataManager.quit(); - } - - // Initialize _snakeWorker. - _snakeDataManager.setProgress( - _nemoProgress.progress()); - _snakeDataManager.setLineDistance( - _snakeLineDistance.rawValue().toDouble()); - _snakeDataManager.setMinTransectLength( - _snakeMinTransectLength.rawValue().toDouble()); - _snakeDataManager.setTileHeight( - _snakeTileHeight.rawValue().toDouble()); - _snakeDataManager.setTileWidth( - _snakeTileWidth.rawValue().toDouble()); - _snakeDataManager.setMinTileArea( - _snakeMinTileArea.rawValue().toDouble()); - _setSnakeCalcInProgress(true); - - // Start worker thread. - _snakeDataManager.start(); -} - -void WimaController::_switchSnakeManager(QVariant variant) -{ - if (variant.value()){ - _switchWaypointManager(_snakeManager); - } else { - _switchWaypointManager(_defaultManager); - } +void WimaController::_progressChangedHandler() { + emit this->nemoProgressChanged(); + this->_currentDM->start(); } - +void WimaController::_enableSnakeChangedHandler() { + if (this->_enableSnake.rawValue().toBool()) { + this->_snakeDM.enableRosBridge(); + _switchDataManager(_snakeDM); + _currentDM->start(); + } else { + this->_snakeDM.disableRosBride(); + _switchDataManager(_emptyDM); + } +} diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index b13e30c14a4e6f41fbdd25458b8158ab498ded48..ba8e3ea3b4c24c0ce3865de8a969326d90d95303 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -7,32 +7,29 @@ #include "QmlObjectListModel.h" #include "Geometry/WimaArea.h" -#include "Geometry/WimaMeasurementArea.h" -#include "Geometry/WimaServiceArea.h" #include "Geometry/WimaCorridor.h" -#include "Geometry/WimaMeasurementAreaData.h" #include "Geometry/WimaCorridorData.h" +#include "Geometry/WimaMeasurementArea.h" +#include "Geometry/WimaMeasurementAreaData.h" +#include "Geometry/WimaServiceArea.h" #include "Geometry/WimaServiceAreaData.h" #include "WimaPlanData.h" -#include "PlanMasterController.h" +#include "JsonHelper.h" #include "MissionController.h" -#include "SurveyComplexItem.h" -#include "SimpleMissionItem.h" #include "MissionSettingsItem.h" -#include "JsonHelper.h" +#include "PlanMasterController.h" #include "QGCApplication.h" #include "SettingsFact.h" #include "SettingsManager.h" +#include "SimpleMissionItem.h" +#include "SurveyComplexItem.h" -#include "snake.h" +#include "Geometry/GeoPoint3D.h" #include "Snake/SnakeDataManager.h" #include "Snake/SnakeTiles.h" #include "Snake/SnakeTilesLocal.h" -#include "Geometry/GeoPoint3D.h" - -#include "ros_bridge/include/ros_bridge.h" #include "WaypointManager/DefaultManager.h" #include "WaypointManager/RTLManager.h" @@ -41,363 +38,285 @@ #include - -using namespace snake; - typedef std::unique_ptr JsonDocUPtr; -class WimaController : public QObject -{ - Q_OBJECT - enum FileType {WimaFile, PlanFile}; -public: - - - WimaController(QObject *parent = nullptr); +class WimaController : public QObject { + Q_OBJECT + enum FileType { WimaFile, PlanFile }; - // Controllers. - Q_PROPERTY(PlanMasterController* masterController - READ masterController - WRITE setMasterController - NOTIFY masterControllerChanged - ) - Q_PROPERTY(MissionController* missionController - READ missionController - WRITE setMissionController - NOTIFY missionControllerChanged - ) - // Wima Data. - Q_PROPERTY(QmlObjectListModel* visualItems - READ visualItems - NOTIFY visualItemsChanged - ) - Q_PROPERTY(QmlObjectListModel* missionItems - READ missionItems - NOTIFY missionItemsChanged - ) - Q_PROPERTY(QmlObjectListModel* currentMissionItems - READ currentMissionItems - NOTIFY currentMissionItemsChanged - ) - Q_PROPERTY(QVariantList waypointPath - READ waypointPath - NOTIFY waypointPathChanged - ) - Q_PROPERTY(QVariantList currentWaypointPath - READ currentWaypointPath - NOTIFY currentWaypointPathChanged - ) - Q_PROPERTY(Fact* enableWimaController - READ enableWimaController - CONSTANT) - // Waypoint navigaton. - Q_PROPERTY(Fact* overlapWaypoints - READ overlapWaypoints - CONSTANT - ) - Q_PROPERTY(Fact* maxWaypointsPerPhase - READ maxWaypointsPerPhase - CONSTANT - ) - Q_PROPERTY(Fact* startWaypointIndex - READ startWaypointIndex - CONSTANT - ) - Q_PROPERTY(Fact* showAllMissionItems - READ showAllMissionItems - CONSTANT - ) - Q_PROPERTY(Fact* showCurrentMissionItems - READ showCurrentMissionItems - CONSTANT - ) - // Waypoint settings. - Q_PROPERTY(Fact* flightSpeed - READ flightSpeed - CONSTANT - ) - Q_PROPERTY(Fact* altitude - READ altitude - CONSTANT - ) - Q_PROPERTY(Fact* arrivalReturnSpeed - READ arrivalReturnSpeed - CONSTANT - ) - // Waypoint statistics. - Q_PROPERTY(double phaseDistance - READ phaseDistance - NOTIFY phaseDistanceChanged - ) - Q_PROPERTY(double phaseDuration - READ phaseDuration - NOTIFY phaseDurationChanged - ) - - // Snake - Q_PROPERTY(Fact* enableSnake - READ enableSnake - CONSTANT - ) - Q_PROPERTY(int nemoStatus - READ nemoStatus - NOTIFY nemoStatusChanged - ) - Q_PROPERTY(QString nemoStatusString - READ nemoStatusString - NOTIFY nemoStatusStringChanged - ) - Q_PROPERTY(bool snakeCalcInProgress - READ snakeCalcInProgress - NOTIFY snakeCalcInProgressChanged - ) - Q_PROPERTY(Fact* snakeTileWidth - READ snakeTileWidth - CONSTANT - ) - Q_PROPERTY(Fact* snakeTileHeight - READ snakeTileHeight - CONSTANT - ) - Q_PROPERTY(Fact* snakeMinTileArea - READ snakeMinTileArea - CONSTANT - ) - Q_PROPERTY(Fact* snakeLineDistance - READ snakeLineDistance - CONSTANT - ) - Q_PROPERTY(Fact* snakeMinTransectLength - READ snakeMinTransectLength - CONSTANT - ) - Q_PROPERTY(QmlObjectListModel* snakeTiles - READ snakeTiles - NOTIFY snakeTilesChanged - ) - Q_PROPERTY(QVariantList snakeTileCenterPoints - READ snakeTileCenterPoints - NOTIFY snakeTileCenterPointsChanged - ) - Q_PROPERTY(QVector nemoProgress - READ nemoProgress - NOTIFY nemoProgressChanged - ) - - - - // Property accessors - // Controllers. - PlanMasterController* masterController (void); - MissionController* missionController (void); - // Wima Data - QmlObjectListModel* visualItems (void); - QGCMapPolygon joinedArea (void) const; - // Waypoints. - QmlObjectListModel* missionItems (void); - QmlObjectListModel* currentMissionItems (void); - QVariantList waypointPath (void); - QVariantList currentWaypointPath (void); - // Settings facts. - Fact* enableWimaController (void); - Fact* overlapWaypoints (void); - Fact* maxWaypointsPerPhase (void); - Fact* startWaypointIndex (void); - Fact* showAllMissionItems (void); - Fact* showCurrentMissionItems(void); - Fact* flightSpeed (void); - Fact* arrivalReturnSpeed (void); - Fact* altitude (void); - // Snake settings facts. - Fact* enableSnake (void) { return &_enableSnake; } - Fact* snakeTileWidth (void) { return &_snakeTileWidth;} - Fact* snakeTileHeight (void) { return &_snakeTileHeight;} - Fact* snakeMinTileArea (void) { return &_snakeMinTileArea;} - Fact* snakeLineDistance (void) { return &_snakeLineDistance;} - Fact* snakeMinTransectLength (void) { return &_snakeMinTransectLength;} - // Snake data. - QmlObjectListModel* snakeTiles (void); - QVariantList snakeTileCenterPoints (void); - QVector nemoProgress (void); - int nemoStatus (void) const; - QString nemoStatusString (void) const; - bool snakeCalcInProgress (void) const; - - // Smart RTL. - bool uploadOverrideRequired (void) const; - bool vehicleHasLowBattery (void) const; - // Waypoint statistics. - double phaseDistance (void) const; - double phaseDuration (void) const; - - - // Property setters - void setMasterController (PlanMasterController* masterController); - void setMissionController (MissionController* missionController); - bool setWimaPlanData (const WimaPlanData &planData); - - // Member Methodes - Q_INVOKABLE WimaController *thisPointer(); - // Waypoint navigation. - Q_INVOKABLE void nextPhase(); - Q_INVOKABLE void previousPhase(); - Q_INVOKABLE void resetPhase(); - // Smart RTL. - Q_INVOKABLE void requestSmartRTL(); - Q_INVOKABLE void initSmartRTL(); - Q_INVOKABLE void executeSmartRTL(); - // Other. - Q_INVOKABLE void removeVehicleTrajectoryHistory(); - Q_INVOKABLE bool upload(); - Q_INVOKABLE bool forceUpload(); - Q_INVOKABLE void removeFromVehicle(); - - - // static Members - static const char* areaItemsName; - static const char* missionItemsName; - static const char* settingsGroup; - static const char* endWaypointIndexName; - static const char* enableWimaControllerName; - static const char* overlapWaypointsName; - static const char* maxWaypointsPerPhaseName; - static const char* startWaypointIndexName; - static const char* showAllMissionItemsName; - static const char* showCurrentMissionItemsName; - static const char* flightSpeedName; - static const char* arrivalReturnSpeedName; - static const char* altitudeName; - static const char* snakeTileWidthName; - static const char* snakeTileHeightName; - static const char* snakeMinTileAreaName; - static const char* snakeLineDistanceName; - static const char* snakeMinTransectLengthName; +public: + WimaController(QObject *parent = nullptr); + + // Controllers. + Q_PROPERTY(PlanMasterController *masterController READ masterController WRITE + setMasterController NOTIFY masterControllerChanged) + Q_PROPERTY(MissionController *missionController READ missionController WRITE + setMissionController NOTIFY missionControllerChanged) + // Wima Data. + Q_PROPERTY(QmlObjectListModel *visualItems READ visualItems NOTIFY + visualItemsChanged) + Q_PROPERTY(QmlObjectListModel *missionItems READ missionItems NOTIFY + missionItemsChanged) + Q_PROPERTY(QmlObjectListModel *currentMissionItems READ currentMissionItems + NOTIFY currentMissionItemsChanged) + Q_PROPERTY( + QVariantList waypointPath READ waypointPath NOTIFY waypointPathChanged) + Q_PROPERTY(QVariantList currentWaypointPath READ currentWaypointPath NOTIFY + currentWaypointPathChanged) + Q_PROPERTY(Fact *enableWimaController READ enableWimaController CONSTANT) + // Waypoint navigaton. + Q_PROPERTY(Fact *overlapWaypoints READ overlapWaypoints CONSTANT) + Q_PROPERTY(Fact *maxWaypointsPerPhase READ maxWaypointsPerPhase CONSTANT) + Q_PROPERTY(Fact *startWaypointIndex READ startWaypointIndex CONSTANT) + Q_PROPERTY(Fact *showAllMissionItems READ showAllMissionItems CONSTANT) + Q_PROPERTY( + Fact *showCurrentMissionItems READ showCurrentMissionItems CONSTANT) + // Waypoint settings. + Q_PROPERTY(Fact *flightSpeed READ flightSpeed CONSTANT) + Q_PROPERTY(Fact *altitude READ altitude CONSTANT) + Q_PROPERTY(Fact *arrivalReturnSpeed READ arrivalReturnSpeed CONSTANT) + // Waypoint statistics. + Q_PROPERTY( + double phaseDistance READ phaseDistance NOTIFY phaseDistanceChanged) + Q_PROPERTY( + double phaseDuration READ phaseDuration NOTIFY phaseDurationChanged) + + // Snake + Q_PROPERTY(Fact *enableSnake READ enableSnake CONSTANT) + Q_PROPERTY(int nemoStatus READ nemoStatus NOTIFY nemoStatusChanged) + Q_PROPERTY(QString nemoStatusString READ nemoStatusString NOTIFY + nemoStatusStringChanged) + Q_PROPERTY(bool snakeCalcInProgress READ snakeCalcInProgress NOTIFY + snakeCalcInProgressChanged) + Q_PROPERTY(Fact *snakeTileWidth READ snakeTileWidth CONSTANT) + Q_PROPERTY(Fact *snakeTileHeight READ snakeTileHeight CONSTANT) + Q_PROPERTY(Fact *snakeMinTileArea READ snakeMinTileArea CONSTANT) + Q_PROPERTY(Fact *snakeLineDistance READ snakeLineDistance CONSTANT) + Q_PROPERTY(Fact *snakeMinTransectLength READ snakeMinTransectLength CONSTANT) + Q_PROPERTY( + QmlObjectListModel *snakeTiles READ snakeTiles NOTIFY snakeTilesChanged) + Q_PROPERTY(QVariantList snakeTileCenterPoints READ snakeTileCenterPoints + NOTIFY snakeTileCenterPointsChanged) + Q_PROPERTY( + QVector nemoProgress READ nemoProgress NOTIFY nemoProgressChanged) + + // Property accessors + // Controllers. + PlanMasterController *masterController(void); + MissionController *missionController(void); + // Wima Data + QmlObjectListModel *visualItems(void); + QGCMapPolygon joinedArea(void) const; + // Waypoints. + QmlObjectListModel *missionItems(void); + QmlObjectListModel *currentMissionItems(void); + QVariantList waypointPath(void); + QVariantList currentWaypointPath(void); + // Settings facts. + Fact *enableWimaController(void); + Fact *overlapWaypoints(void); + Fact *maxWaypointsPerPhase(void); + Fact *startWaypointIndex(void); + Fact *showAllMissionItems(void); + Fact *showCurrentMissionItems(void); + Fact *flightSpeed(void); + Fact *arrivalReturnSpeed(void); + Fact *altitude(void); + // Snake settings facts. + Fact *enableSnake(void) { return &_enableSnake; } + Fact *snakeTileWidth(void) { return &_snakeTileWidth; } + Fact *snakeTileHeight(void) { return &_snakeTileHeight; } + Fact *snakeMinTileArea(void) { return &_snakeMinTileArea; } + Fact *snakeLineDistance(void) { return &_snakeLineDistance; } + Fact *snakeMinTransectLength(void) { return &_snakeMinTransectLength; } + // Snake data. + QmlObjectListModel *snakeTiles(void); + QVariantList snakeTileCenterPoints(void); + QVector nemoProgress(void); + int nemoStatus(void) const; + QString nemoStatusString(void) const; + bool snakeCalcInProgress(void) const; + + // Smart RTL. + bool uploadOverrideRequired(void) const; + bool vehicleHasLowBattery(void) const; + // Waypoint statistics. + double phaseDistance(void) const; + double phaseDuration(void) const; + + // Property setters + void setMasterController(PlanMasterController *masterController); + void setMissionController(MissionController *missionController); + bool setWimaPlanData(const WimaPlanData &planData); + + // Member Methodes + Q_INVOKABLE WimaController *thisPointer(); + // Waypoint navigation. + Q_INVOKABLE void nextPhase(); + Q_INVOKABLE void previousPhase(); + Q_INVOKABLE void resetPhase(); + // Smart RTL. + Q_INVOKABLE void requestSmartRTL(); + Q_INVOKABLE void initSmartRTL(); + Q_INVOKABLE void executeSmartRTL(); + // Other. + Q_INVOKABLE void removeVehicleTrajectoryHistory(); + Q_INVOKABLE bool upload(); + Q_INVOKABLE bool forceUpload(); + Q_INVOKABLE void removeFromVehicle(); + + // static Members + static const char *areaItemsName; + static const char *missionItemsName; + static const char *settingsGroup; + static const char *endWaypointIndexName; + static const char *enableWimaControllerName; + static const char *overlapWaypointsName; + static const char *maxWaypointsPerPhaseName; + static const char *startWaypointIndexName; + static const char *showAllMissionItemsName; + static const char *showCurrentMissionItemsName; + static const char *flightSpeedName; + static const char *arrivalReturnSpeedName; + static const char *altitudeName; + static const char *snakeTileWidthName; + static const char *snakeTileHeightName; + static const char *snakeMinTileAreaName; + static const char *snakeLineDistanceName; + static const char *snakeMinTransectLengthName; signals: - // Controllers. - void masterControllerChanged (void); - void missionControllerChanged (void); - // Wima data. - void visualItemsChanged (void); - // Waypoints. - void missionItemsChanged (void); - void currentMissionItemsChanged (void); - void waypointPathChanged (void); - void currentWaypointPathChanged (void); - // Smart RTL. - void smartRTLRequestConfirm (void); - void smartRTLPathConfirm (void); - // Upload. - void forceUploadConfirm (void); - // Waypoint statistics. - void phaseDistanceChanged (void); - void phaseDurationChanged (void); - // Snake. - void snakeConnectionStatusChanged (void); - void snakeCalcInProgressChanged (void); - void snakeTilesChanged (void); - void snakeTileCenterPointsChanged (void); - void nemoProgressChanged (void); - void nemoStatusChanged (void); - void nemoStatusStringChanged (void); + // Controllers. + void masterControllerChanged(void); + void missionControllerChanged(void); + // Wima data. + void visualItemsChanged(void); + // Waypoints. + void missionItemsChanged(void); + void currentMissionItemsChanged(void); + void waypointPathChanged(void); + void currentWaypointPathChanged(void); + // Smart RTL. + void smartRTLRequestConfirm(void); + void smartRTLPathConfirm(void); + // Upload. + void forceUploadConfirm(void); + // Waypoint statistics. + void phaseDistanceChanged(void); + void phaseDurationChanged(void); + // Snake. + void snakeConnectionStatusChanged(void); + void snakeCalcInProgressChanged(void); + void snakeTilesChanged(void); + void snakeTileCenterPointsChanged(void); + void nemoProgressChanged(void); + void nemoStatusChanged(void); + void nemoStatusStringChanged(void); private slots: - // Waypoint navigation / helper. - bool _calcNextPhase (void); - void _recalcCurrentPhase (void); - bool _calcShortestPath (const QGeoCoordinate &start, - const QGeoCoordinate &destination, - QVector &path); - // Slicing parameters - bool _setStartIndex (void); - void _updateOverlap (void); - void _updateMaxWaypoints (void); - // Waypoint settings. - void _updateflightSpeed (void); - void _updateArrivalReturnSpeed (void); - void _updateAltitude (void); - // Waypoint Statistics. - void _setPhaseDistance (double distance); - void _setPhaseDuration (double duration); - // SMART RTL - void _checkBatteryLevel (void); - bool _checkSmartRTLPreCondition (QString &errorString); - void _initSmartRTL (); - void _smartRTLCleanUp (bool flying); - // Snake. - void _setSnakeCalcInProgress (bool inProgress); - void _snakeStoreWorkerResults (); - void _initStartSnakeWorker (); - void _switchSnakeManager (QVariant variant); - bool _doTopicServiceSetup(); - // Periodic tasks. - void _eventTimerHandler (void); - // Waypoint manager handling. - void _switchWaypointManager(WaypointManager::ManagerBase &manager); + // Waypoint navigation / helper. + bool _calcNextPhase(void); + void _recalcCurrentPhase(void); + bool _calcShortestPath(const QGeoCoordinate &start, + const QGeoCoordinate &destination, + QVector &path); + // Slicing parameters + bool _setStartIndex(void); + void _updateOverlap(void); + void _updateMaxWaypoints(void); + // Waypoint settings. + void _updateflightSpeed(void); + void _updateArrivalReturnSpeed(void); + void _updateAltitude(void); + // Waypoint Statistics. + void _setPhaseDistance(double distance); + void _setPhaseDuration(double duration); + // SMART RTL + void _checkBatteryLevel(void); + bool _checkSmartRTLPreCondition(QString &errorString); + void _initSmartRTL(); + void _smartRTLCleanUp(bool flying); + // Snake. + void _DMFinishedHandler(); + void _switchToSnakeWaypointManager(QVariant variant); + void _switchDataManager(SnakeDataManager &dataManager); + void _progressChangedHandler(); + void _enableSnakeChangedHandler(); + // Periodic tasks. + void _eventTimerHandler(void); + // Waypoint manager handling. + void _switchWaypointManager(WaypointManager::ManagerBase &manager); private: - using StatusMap = std::map; - - // Controllers. - PlanMasterController *_masterController; - MissionController *_missionController; - - // Wima Data. - QmlObjectListModel _areas; // contains all visible areas - WimaJoinedAreaData _joinedArea; // joined area fromed by opArea, serArea, _corridor - WimaMeasurementAreaData _measurementArea; // measurement area - WimaServiceAreaData _serviceArea; // area for supplying - WimaCorridorData _corridor; // corridor connecting opArea and serArea - bool _localPlanDataValid; - - // Waypoint Managers. - WaypointManager::AreaInterface _areaInterface; - WaypointManager::Settings _managerSettings; - WaypointManager::DefaultManager _defaultManager; - WaypointManager::DefaultManager _snakeManager; - WaypointManager::RTLManager _rtlManager; - WaypointManager::ManagerBase *_currentManager; - using ManagerList = QList; - ManagerList _managerList; - - // Settings Facts. - QMap _metaDataMap; - SettingsFact _enableWimaController; // enables or disables the wimaControler - SettingsFact _overlapWaypoints; // determines the number of overlapping waypoints between two consecutive mission phases - SettingsFact _maxWaypointsPerPhase; // determines the maximum number waypoints per phase - SettingsFact _nextPhaseStartWaypointIndex; // index (displayed on the map, -1 to get index of item in _missionItems) of the mission item - // defining the first element of the next phase - SettingsFact _showAllMissionItems; // bool value, Determines whether the mission items of the overall mission are displayed or not. - SettingsFact _showCurrentMissionItems; // bool value, Determines whether the mission items of the current mission phase are displayed or not. - SettingsFact _flightSpeed; // mission flight speed - SettingsFact _arrivalReturnSpeed; // arrival and return path speed - SettingsFact _altitude; // mission altitude - SettingsFact _enableSnake; // Enable Snake (see snake.h) - SettingsFact _snakeTileWidth; - SettingsFact _snakeTileHeight; - SettingsFact _snakeMinTileArea; - SettingsFact _snakeLineDistance; - SettingsFact _snakeMinTransectLength; - - // Smart RTL. - QTimer _smartRTLTimer; - bool _lowBatteryHandlingTriggered; - - // Waypoint statistics. - double _measurementPathLength; // the lenght of the phase in meters - - // Snake - SnakeDataManager _snakeDataManager; - int _fallbackStatus; - static StatusMap _nemoStatusMap; - - // Periodic tasks. - QTimer _eventTimer; - EventTicker _batteryLevelTicker; - EventTicker _snakeTicker; - EventTicker _nemoTimeoutTicker; - + using StatusMap = std::map; + + // Controllers. + PlanMasterController *_masterController; + MissionController *_missionController; + + // Wima Data. + QmlObjectListModel _areas; // contains all visible areas + WimaJoinedAreaData + _joinedArea; // joined area fromed by opArea, serArea, _corridor + WimaMeasurementAreaData _measurementArea; // measurement area + WimaServiceAreaData _serviceArea; // area for supplying + WimaCorridorData _corridor; // corridor connecting opArea and serArea + bool _localPlanDataValid; + + // Waypoint Managers. + WaypointManager::AreaInterface _areaInterface; + WaypointManager::Settings _WMSettings; // Waypoint Manager Settings + WaypointManager::DefaultManager _defaultWM; + WaypointManager::DefaultManager _snakeWM; + WaypointManager::RTLManager _rtlWM; + WaypointManager::ManagerBase *_currentWM; + using ManagerList = QList; + ManagerList _WMList; + + // Settings Facts. + QMap _metaDataMap; + SettingsFact _enableWimaController; // enables or disables the wimaControler + SettingsFact + _overlapWaypoints; // determines the number of overlapping waypoints + // between two consecutive mission phases + SettingsFact _maxWaypointsPerPhase; // determines the maximum number waypoints + // per phase + SettingsFact + _nextPhaseStartWaypointIndex; // index (displayed on the map, -1 to get + // index of item in _missionItems) of the + // mission item defining the first element + // of the next phase + SettingsFact + _showAllMissionItems; // bool value, Determines whether the mission items + // of the overall mission are displayed or not. + SettingsFact _showCurrentMissionItems; // bool value, Determines whether the + // mission items of the current mission + // phase are displayed or not. + SettingsFact _flightSpeed; // mission flight speed + SettingsFact _arrivalReturnSpeed; // arrival and return path speed + SettingsFact _altitude; // mission altitude + SettingsFact _enableSnake; // Enable Snake (see snake.h) + SettingsFact _snakeTileWidth; + SettingsFact _snakeTileHeight; + SettingsFact _snakeMinTileArea; + SettingsFact _snakeLineDistance; + SettingsFact _snakeMinTransectLength; + + // Smart RTL. + QTimer _smartRTLTimer; + bool _lowBatteryHandlingTriggered; + + // Waypoint statistics. + double _measurementPathLength; // the lenght of the phase in meters + + // Snake + SnakeDataManager _snakeDM; // Snake Data Manager + SnakeDataManager _emptyDM; + SnakeDataManager *_currentDM; + static StatusMap _nemoStatusMap; + + // Periodic tasks. + QTimer _eventTimer; + EventTicker _batteryLevelTicker; }; - - diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h b/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h index 6f53c59af9bc4b038205410d30e93795cdd28630..c244688a5a7c53f32f853ec8d0c249e53ea845a2 100644 --- a/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h +++ b/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h @@ -17,7 +17,7 @@ std::string messageType(); //! @brief C++ representation of nemo_msgs/Heartbeat message class Heartbeat{ public: - Heartbeat(){} + Heartbeat() : _status(0){} Heartbeat(int status) :_status(status){} Heartbeat(const Heartbeat &heartbeat) : _status(heartbeat.status()){}