From 7c0976d98835d7a09b27a3ab74f8ebf524ecef13 Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Mon, 3 Aug 2020 18:32:23 +0200 Subject: [PATCH] not compilable --- qgroundcontrol.pro | 4 - src/Snake/flight_plan.cpp | 372 ---------------------- src/Snake/flight_plan.h | 59 ---- src/Snake/snake.cpp | 578 +++++++++++++++++++++++++--------- src/Snake/snake.h | 138 ++++++-- src/Snake/snake_geometry.cpp | 1 - src/Snake/snake_geometry.h | 0 src/Wima/Snake/SnakeWorker.cc | 309 +++++++++++------- src/Wima/Snake/SnakeWorker.h | 104 +++--- src/Wima/WimaController.cc | 4 +- src/Wima/WimaController.h | 2 +- 11 files changed, 786 insertions(+), 785 deletions(-) delete mode 100644 src/Snake/flight_plan.cpp delete mode 100644 src/Snake/flight_plan.h delete mode 100644 src/Snake/snake_geometry.cpp delete mode 100644 src/Snake/snake_geometry.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 622684a8e6..80ef0ac43b 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -408,7 +408,6 @@ FORMS += \ HEADERS += \ src/Snake/clipper/clipper.hpp \ - src/Snake/flight_plan.h \ src/Snake/mapbox/feature.hpp \ src/Snake/mapbox/geometry.hpp \ src/Snake/mapbox/geometry/box.hpp \ @@ -430,7 +429,6 @@ HEADERS += \ src/Snake/mapbox/variant.hpp \ src/Snake/mapbox/variant_io.hpp \ src/Snake/snake.h \ - src/Snake/snake_geometry.h \ src/Snake/snake_typedefs.h \ src/Wima/Geometry/GeoPoint3D.h \ src/Wima/Geometry/Polygon2D.h \ @@ -498,9 +496,7 @@ HEADERS += \ src/comm/utilities.h SOURCES += \ src/Snake/clipper/clipper.cpp \ - src/Snake/flight_plan.cpp \ src/Snake/snake.cpp \ - src/Snake/snake_geometry.cpp \ src/Wima/Geometry/GeoPoint3D.cpp \ src/Wima/Geometry/PolygonArray.cc \ src/Wima/Snake/GeoTile.cpp \ diff --git a/src/Snake/flight_plan.cpp b/src/Snake/flight_plan.cpp deleted file mode 100644 index ae9aa47e13..0000000000 --- a/src/Snake/flight_plan.cpp +++ /dev/null @@ -1,372 +0,0 @@ -#include "flight_plan.h" - -#include "clipper/clipper.hpp" -#define CLIPPER_SCALE 10000 - -#include "ortools/constraint_solver/routing.h" -#include "ortools/constraint_solver/routing_enums.pb.h" -#include "ortools/constraint_solver/routing_index_manager.h" -#include "ortools/constraint_solver/routing_parameters.h" - - - - -struct FlightPlan::RoutingDataModel{ - Matrix distanceMatrix; - long numVehicles; - RoutingIndexManager::NodeIndex depot; -}; - -FlightPlan::FlightPlan() -{ - -} - -bool FlightPlan::generate(double lineDistance, double minTransectLength) -{ - _waypointsENU.clear(); - _waypoints.clear(); - _arrivalPathIdx.clear(); - _returnPathIdx.clear(); - -#ifndef NDEBUG - _PathVertices.clear(); -#endif -#ifdef SHOW_TIME - auto start = std::chrono::high_resolution_clock::now(); -#endif - if (!_generateTransects(lineDistance, minTransectLength)) - return false; -#ifdef SHOW_TIME - auto delta = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start); - cout << endl; - cout << "Execution time _generateTransects(): " << delta.count() << " ms" << endl; -#endif - - //======================================= - // Route Transects using Google or-tools. - //======================================= - // Offset joined area. - const BoostPolygon &jArea = _scenario.getJoineAreaENU(); - BoostPolygon jAreaOffset; - offsetPolygon(jArea, jAreaOffset, detail::offsetConstant); - - // Create vertex list; - BoostLineString vertices; - size_t n_t = _transects.size()*2; - size_t n0 = n_t+1; - vertices.reserve(n0); - for (auto lstring : _transects){ - for (auto vertex : lstring){ - vertices.push_back(vertex); - } - } - vertices.push_back(_scenario.getHomePositonENU()); - - for (long i=0; i connectionGraph(n1, n1); - -#ifdef SHOW_TIME - start = std::chrono::high_resolution_clock::now(); -#endif - _generateRoutingModel(vertices, jAreaOffset, n0, dataModel, connectionGraph); -#ifdef SHOW_TIME - delta = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start); - cout << "Execution time _generateRoutingModel(): " << delta.count() << " ms" << endl; -#endif - - // Create Routing Index Manager. - RoutingIndexManager manager(dataModel.distanceMatrix.getN(), dataModel.numVehicles, - dataModel.depot); - // Create Routing Model. - RoutingModel routing(manager); - - // Create and register a transit callback. - const int transit_callback_index = routing.RegisterTransitCallback( - [&dataModel, &manager](int64 from_index, int64 to_index) -> int64 { - // Convert from routing variable Index to distance matrix NodeIndex. - auto from_node = manager.IndexToNode(from_index).value(); - auto to_node = manager.IndexToNode(to_index).value(); - return dataModel.distanceMatrix.get(from_node, to_node); - }); - - // Define cost of each arc. - routing.SetArcCostEvaluatorOfAllVehicles(transit_callback_index); - - - // Define Constraints. - size_t n = _transects.size()*2; - Solver *solver = routing.solver(); - for (size_t i=0; iIsEqual(idx1); -// auto cond1 = routing.NextVar(idx1)->IsEqual(idx0); -// auto c = solver->MakeNonEquality(cond0, cond1); -// solver->AddConstraint(c); - - // alternative - auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i)); - auto idx1 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i+1)); - auto cond0 = routing.NextVar(idx0)->IsEqual(idx1); - auto cond1 = routing.NextVar(idx1)->IsEqual(idx0); - vector conds{cond0, cond1}; - auto c = solver->MakeAllDifferent(conds); - solver->MakeRejectFilter(); - solver->AddConstraint(c); - } - - - // Setting first solution heuristic. - RoutingSearchParameters searchParameters = DefaultRoutingSearchParameters(); - searchParameters.set_first_solution_strategy( - FirstSolutionStrategy::PATH_CHEAPEST_ARC); - google::protobuf::Duration *tMax = new google::protobuf::Duration(); // seconds - tMax->set_seconds(10); - searchParameters.set_allocated_time_limit(tMax); - - // Solve the problem. -#ifdef SHOW_TIME - start = std::chrono::high_resolution_clock::now(); -#endif - const Assignment* solution = routing.SolveWithParameters(searchParameters); -#ifdef SHOW_TIME - delta = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start); - cout << "Execution time routing.SolveWithParameters(): " << delta.count() << " ms" << endl; -#endif - - if (!solution || solution->Size() <= 1){ - errorString = "Not able to solve the routing problem."; - return false; - } - - // Extract waypoints from solution. - long index = routing.Start(0); - std::vector route; - route.push_back(manager.IndexToNode(index).value()); - while (!routing.IsEnd(index)){ - index = solution->Value(routing.NextVar(index)); - route.push_back(manager.IndexToNode(index).value()); - } - - // Connect transects -#ifndef NDEBUG - _PathVertices = vertices; -#endif - - { - _waypointsENU.push_back(vertices[route[0]]); - vector pathIdx; - long arrivalPathLength = 0; - for (long i=0; i 0; --i) - _returnPathIdx.push_back(_waypointsENU.size()-i); - - for (long i=0; i < arrivalPathLength; ++i) - _arrivalPathIdx.push_back(i); - } - - // Back transform waypoints. - GeoPoint3D origin{_scenario.getOrigin()}; - for (auto vertex : _waypointsENU) { - GeoPoint3D geoVertex; - fromENU(origin, Point3D{vertex.get<0>(), vertex.get<1>(), 0}, geoVertex); - _waypoints.push_back(GeoPoint2D{geoVertex[0], geoVertex[1]}); - } - - return true; -} - -bool FlightPlan::_generateTransects(double lineDistance, double minTransectLength) -{ - _transects.clear(); - if (_scenario.getTilesENU().size() != _progress.size()){ - ostringstream strstream; - strstream << "Number of tiles (" - << _scenario.getTilesENU().size() - << ") is not equal to progress array length (" - << _progress.size() - << ")"; - errorString = strstream.str(); - return false; - } - - // Calculate processed tiles (_progress[i] == 100) and subtract them from measurement area. - size_t num_tiles = _progress.size(); - vector processedTiles; - { - const auto &tiles = _scenario.getTilesENU(); - for (size_t i=0; i(vertex.get<0>()*CLIPPER_SCALE), - static_cast(vertex.get<1>()*CLIPPER_SCALE)}); - } - vector processedTilesClipper; - for (auto t : processedTiles){ - ClipperLib::Path path; - for (auto vertex : t.outer()){ - path.push_back(ClipperLib::IntPoint{static_cast(vertex.get<0>()*CLIPPER_SCALE), - static_cast(vertex.get<1>()*CLIPPER_SCALE)}); - } - processedTilesClipper.push_back(path); - } - - const min_bbox_rt &bbox = _scenario.getMeasurementAreaBBoxENU(); - double alpha = bbox.angle; - double x0 = bbox.corners.outer()[0].get<0>(); - double y0 = bbox.corners.outer()[0].get<1>(); - double bboxWidth = bbox.width; - double bboxHeight = bbox.height; - double delta = detail::offsetConstant; - - size_t num_t = int(ceil((bboxHeight + 2*delta)/lineDistance)); // number of transects - vector yCoords; - yCoords.reserve(num_t); - double y = -delta; - for (size_t i=0; i < num_t; ++i) { - yCoords.push_back(y); - y += lineDistance; - } - - - // Generate transects and convert them to clipper path. - trans::rotate_transformer rotate_back(-alpha*180/M_PI); - trans::translate_transformer translate_back(x0, y0); - vector transectsClipper; - transectsClipper.reserve(num_t); - for (size_t i=0; i < num_t; ++i) { - // calculate transect - BoostPoint v1{-delta, yCoords[i]}; - BoostPoint v2{bboxWidth+delta, yCoords[i]}; - BoostLineString transect; - transect.push_back(v1); - transect.push_back(v2); - - // transform back - BoostLineString temp_transect; - bg::transform(transect, temp_transect, rotate_back); - transect.clear(); - bg::transform(temp_transect, transect, translate_back); - - - ClipperLib::IntPoint c1{static_cast(transect[0].get<0>()*CLIPPER_SCALE), - static_cast(transect[0].get<1>()*CLIPPER_SCALE)}; - ClipperLib::IntPoint c2{static_cast(transect[1].get<0>()*CLIPPER_SCALE), - static_cast(transect[1].get<1>()*CLIPPER_SCALE)}; - ClipperLib::Path path{c1, c2}; - transectsClipper.push_back(path); - } - - // Perform clipping. - // Clip transects to measurement area. - ClipperLib::Clipper clipper; - clipper.AddPath(mAreaClipper, ClipperLib::ptClip, true); - clipper.AddPaths(transectsClipper, ClipperLib::ptSubject, false); - ClipperLib::PolyTree clippedTransecsPolyTree1; - clipper.Execute(ClipperLib::ctIntersection, clippedTransecsPolyTree1, ClipperLib::pftNonZero, ClipperLib::pftNonZero); - - // Subtract holes (tiles with measurement_progress == 100) from transects. - clipper.Clear(); - for (auto child : clippedTransecsPolyTree1.Childs) - clipper.AddPath(child->Contour, ClipperLib::ptSubject, false); - clipper.AddPaths(processedTilesClipper, ClipperLib::ptClip, true); - ClipperLib::PolyTree clippedTransecsPolyTree2; - clipper.Execute(ClipperLib::ctDifference, clippedTransecsPolyTree2, ClipperLib::pftNonZero, ClipperLib::pftNonZero); - - // Extract transects from PolyTree and convert them to BoostLineString - for (auto child : clippedTransecsPolyTree2.Childs){ - ClipperLib::Path clipperTransect = child->Contour; - BoostPoint v1{static_cast(clipperTransect[0].X)/CLIPPER_SCALE, - static_cast(clipperTransect[0].Y)/CLIPPER_SCALE}; - BoostPoint v2{static_cast(clipperTransect[1].X)/CLIPPER_SCALE, - static_cast(clipperTransect[1].Y)/CLIPPER_SCALE}; - - BoostLineString transect{v1, v2}; - if (bg::length(transect) >= minTransectLength) - _transects.push_back(transect); - - } - - if (_transects.size() < 1) - return false; - - return true; -} - -void FlightPlan::_generateRoutingModel(const BoostLineString &vertices, - const BoostPolygon &polygonOffset, - size_t n0, - FlightPlan::RoutingDataModel &dataModel, - Matrix &graph) -{ -#ifdef SHOW_TIME - auto start = std::chrono::high_resolution_clock::now(); -#endif - graphFromPolygon(polygonOffset, vertices, graph); -#ifdef SHOW_TIME - auto delta = std::chrono::duration_cast(std::chrono::high_resolution_clock::now()-start); - cout << "Execution time graphFromPolygon(): " << delta.count() << " ms" << endl; -#endif -// cout << endl; -// for (size_t i=0; i &row = graph[i]; -// for (size_t j=0; j distanceMatrix(graph); -#ifdef SHOW_TIME - start = std::chrono::high_resolution_clock::now(); -#endif - toDistanceMatrix(distanceMatrix); -#ifdef SHOW_TIME - delta = std::chrono::duration_cast(std::chrono::high_resolution_clock::now()-start); - cout << "Execution time toDistanceMatrix(): " << delta.count() << " ms" << endl; -#endif - - dataModel.distanceMatrix.setDimension(n0, n0); - for (size_t i=0; i &progress) {_progress = progress;} - - const Scenario &getScenario(void) const {return _scenario;} - const BoostLineString &getWaypointsENU(void) const {return _waypointsENU;} - const GeoPoint2DList &getWaypoints(void) const {return _waypoints;} - const vector &getArrivalPathIdx(void) const {return _arrivalPathIdx;} - const vector &getReturnPathIdx(void) const {return _returnPathIdx;} -#ifndef NDEBUG - const BoostLineString &getPathVertices(void) const {return _PathVertices;} -#endif - - bool generate(double lineDistance, double minTransectLength); - const vector &getTransects() const {return _transects;} - - string errorString; - -private: - // Search Filter to speed up routing.SolveWithParameters(...); - // Found here: http://www.lia.disi.unibo.it/Staff/MicheleLombardi/or-tools-doc/user_manual/manual/ls/ls_filtering.html - class SearchFilter; - - struct RoutingDataModel; - - bool _generateTransects(double lineDistance, double minTransectLength); - void _generateRoutingModel(const BoostLineString &vertices, - const BoostPolygon &polygonOffset, - size_t n0, - RoutingDataModel &dataModel, - Matrix &graph); - - Scenario _scenario; - BoostLineString _waypointsENU; - GeoPoint2DList _waypoints; - vector _transects; - vector _progress; - vector _arrivalPathIdx; - vector _returnPathIdx; - -#ifndef NDEBUG - BoostLineString _PathVertices; -#endif -}; - -} diff --git a/src/Snake/snake.cpp b/src/Snake/snake.cpp index 1098ea3d2c..b7ae331b9c 100644 --- a/src/Snake/snake.cpp +++ b/src/Snake/snake.cpp @@ -9,6 +9,14 @@ #include #include +#include "clipper/clipper.hpp" +#define CLIPPER_SCALE 10000 + +#include "ortools/constraint_solver/routing.h" +#include "ortools/constraint_solver/routing_enums.pb.h" +#include "ortools/constraint_solver/routing_index_manager.h" +#include "ortools/constraint_solver/routing_parameters.h" + using namespace operations_research; #ifndef NDEBUG @@ -187,68 +195,6 @@ void minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox) minBBox.corners = rotated_polygon; } -void toBoost(const Point2D &point, BoostPoint &boost_point) -{ - boost_point.set<0>(point[0]); - boost_point.set<1>(point[1]); -} - -void fromBoost(const BoostPoint &boost_point, Point2D &point) -{ - point[0] = boost_point.get<0>(); - point[1] = boost_point.get<1>(); -} - -void toBoost(const Point2DList &point_list, BoostPolygon &boost_polygon) -{ - for (auto vertex : point_list) { - BoostPoint boost_vertex; - toBoost(vertex, boost_vertex); - boost_polygon.outer().push_back(boost_vertex); - } - bg::correct(boost_polygon); -} - -void fromBoost(const BoostPolygon &boost_polygon, Point2DList &point_list) -{ - for (auto boost_vertex : boost_polygon.outer()) { - Point2D vertex; - fromBoost(boost_vertex, vertex); - point_list.push_back(vertex); - } -} - -void rotateDeg(const Point2DList &point_list, Point2DList &rotated_point_list, double degree) -{ - trans::rotate_transformer rotate(degree); - BoostPolygon boost_polygon; - toBoost(point_list, boost_polygon); - BoostPolygon rotated_polygon; - bg::transform(boost_polygon, rotated_polygon, rotate); - fromBoost(rotated_polygon, rotated_point_list); -} - -void rotateRad(const Point2DList &point_list, Point2DList &rotated_point_list, double rad) -{ - rotateDeg(point_list, rotated_point_list, rad*180/M_PI); -} - -bool isClockwise(const Point2DList &point_list) -{ - double orientaion = 0; - double len = point_list.size(); - for (long i=0; i < len-1; ++i){ - Point2D v1 = point_list[i]; - Point2D v2 = point_list[i+1]; - orientaion += (v2[0]-v1[0])*(v2[1]+v1[1]); - } - Point2D v1 = point_list[len-1]; - Point2D v2 = point_list[0]; - orientaion += (v2[0]-v1[0])*(v2[1]+v1[1]); - - return orientaion > 0 ? true : false; -} - void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset, double offset) { bg::strategy::buffer::distance_symmetric distance_strategy(offset); @@ -435,37 +381,148 @@ void shortestPathFromGraph(const Matrix &graph, size_t startIndex, size_ (void)ret; } -} - //========================================================================= -// Tile calculation. +// Scenario calculation. //========================================================================= -bool calculateTiles(const BoostPolygon &mArea, - double tileWidth, - double tileHeight, - double minTileArea, - std::vector &tiles, - std::string &errorString) -{ - using namespace snake_geometry; - if (tileWidth <= 0 || tileHeight <= 0 || minTileArea < 0) { +}Scenario::Scenario() : + _tileWidth(5) + , _tileHeight(5) + , _minTileArea(0) + , _needsUpdate(true) +{ + +} + +void Scenario::setMeasurementArea(const BoostPolygon &area) +{ + _needsUpdate = true; + _mArea = area; +} + +void Scenario::setServiceArea(const BoostPolygon &area) +{ + _needsUpdate = true; + _sArea = area; +} + +void Scenario::setCorridor(const BoostPolygon &area) +{ + _needsUpdate = true; + _corridor = area; +} + +BoostPolygon &Scenario::measurementArea() { + _needsUpdate = true; + return _mArea; +} + +BoostPolygon &Scenario::serviceArea() { + _needsUpdate = true; + return _sArea; +} + +BoostPolygon &Scenario::corridor() { + _needsUpdate = true; + return _corridor; +} + +const BoundingBox &Scenario::mAreaBoundingBox() const +{ + return _mAreaBoundingBox; +} + +const BoostPolygon &Scenario::measurementArea() const +{ + return _mArea; +} + +const BoostPolygon &Scenario::serviceArea() const +{ + return _sArea; +} + +const BoostPolygon &Scenario::corridor() const +{ + return _corridor; +} + +const BoostPolygon &Scenario::joinedArea() const { + return _jArea; +} + +const vector &Scenario::tiles() const{ + return _tiles; +} + +const BoostLineString &Scenario::tileCenterPoints() const{ + return _tileCenterPoints; +} + +const BoundingBox &Scenario::measurementAreaBBox() const{ + return _mAreaBoundingBox; +} + +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; + 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 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. + * @param minTileArea The minimal area (>0) of a tile. + * + * @return Returns true if successful. + */ +bool Scenario::_calculateTiles() +{ + _tiles.clear(); + _tileCenterPoints.clear(); + + if (_tileWidth <= 0 || _tileHeight <= 0 || _minTileArea < 0) { errorString = "Parameters tileWidth, tileHeight, minTileArea must be positive."; return false; } - BoundingBox bbox; - minimalBoundingBox(mArea, bbox); - double bbox_width = bbox.width; - double bbox_height = bbox.height; - BoostPoint origin = bbox.corners.outer()[0]; + double bbox_width = _mAreaBoundingBox.width; + double bbox_height = _mAreaBoundingBox.height; + + BoostPoint origin = _mAreaBoundingBox.corners.outer()[0]; //cout << "Origin: " << origin[0] << " " << origin[1] << endl; - // Transform _measurementAreaENU polygon to bounding box coordinate system. - trans::rotate_transformer rotate(bbox.angle*180/M_PI); + // 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(_measurementAreaENU, translated_polygon, translate); + boost::geometry::transform(_mArea, translated_polygon, translate); boost::geometry::transform(translated_polygon, rotated_polygon, rotate); bg::correct(rotated_polygon); @@ -475,11 +532,12 @@ bool calculateTiles(const BoostPolygon &mArea, size_t j_max = ceil(bbox_height/tileHeight); if (i_max < 1 || j_max < 1) { - errorString = "tileWidth or tileHeight to large."; + errorString = "Tile width or Tile height to large."; return false; } - trans::rotate_transformer rotate_back(-bbox.angle*180/M_PI); + + 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 < i_max; ++i){ double x_min = tileWidth*i; @@ -510,13 +568,16 @@ bool calculateTiles(const BoostPolygon &mArea, boost::geometry::transform(rotated_tile, translated_tile, translate_back); // Store tile and calculate center point. - tiles.push_back(translated_tile); + _tiles.push_back(translated_tile); + BoostPoint tile_center; + polygonCenter(translated_tile, tile_center); + _tileCenterPoints.push_back(tile_center); } } } } - if (tiles.size() < 1){ + if (_tiles.size() < 1){ errorString = "No tiles calculated. Is the minTileArea parameter large enough?"; return false; } @@ -524,6 +585,100 @@ bool calculateTiles(const BoostPolygon &mArea, 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(_corridior, _mArea) ) { + // Corridor overlaping with service area? + if ( bg::intersects(_corridior, _sArea) ) { + corridor_is_connection = true; + } + } + } + + // Are areas joinable? + std::deque sol; + BoostPolygon partialArea = _mArea; + if (overlapingSerMeas){ + if(corridor_is_connection){ + bg::union_(partialArea, _corridior, sol); + } + } else if (corridor_is_connection){ + bg::union_(partialArea, _corridior, 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; +} + +double Scenario::minTileArea() const +{ + return _minTileArea; +} + +void Scenario::setMinTileArea(double minTileArea) +{ + if ( minTileArea >= 0){ + _needsUpdate = true; + _minTileArea = minTileArea; + } +} + +double Scenario::tileHeight() const +{ + return _tileHeight; +} + +void Scenario::setTileHeight(double tileHeight) +{ + if ( tileHeight > 0) { + _needsUpdate = true; + _tileHeight = tileHeight; + } +} + +double Scenario::tileWidth() const +{ + return _tileWidth; +} + +void Scenario::setTileWidth(double tileWidth) +{ + if ( tileWidth > 0 ){ + _needsUpdate = true; + _tileWidth = tileWidth; + } +} + +//========================================================================= +// Tile calculation. +//========================================================================= + bool joinAreas(const std::vector &areas, BoostPolygon &joinedArea) { if (areas.size() < 1) @@ -531,15 +686,15 @@ bool joinAreas(const std::vector &areas, BoostPolygon &joinedArea) std::deque idxList; for(size_t i = 1; i < areas.size(); ++i) idxList.push_back(i); - BoostPolygon partialArea = areas[0]; + joinedArea = areas[0]; std::deque sol; while (idxList.size() > 0){ bool success = false; for (auto it = idxList.begin(); it != idxList.end(); ++it){ - bg::union_(partialArea, areas[*it], sol); + bg::union_(joinedArea, areas[*it], sol); if (sol.size() > 0) { - partialArea = sol[0]; + joinedArea = sol[0]; sol.clear; idxList.erase(it); success = true; @@ -549,23 +704,89 @@ bool joinAreas(const std::vector &areas, BoostPolygon &joinedArea) if ( !success ) return false; } + + return true; +} + +BoundingBox::BoundingBox() : + width(0) + , height(0) + , angle(0) +{ + +} + +void BoundingBox::clear() +{ + width = 0; + height = 0; + angle = 0; + corners.clear(); +} + +Flightplan::Flightplan(ScenarioCPtr s, ProgressCPtr p) + : _scenario(s) + , _progress(p) +{ + +} + +double Flightplan::lineDistance() const +{ + return _lineDistance; +} + +void Flightplan::setLineDistance(double lineDistance) +{ + _lineDistance = lineDistance; +} + +double Flightplan::minTransectLength() const +{ + return _minTransectLength; +} + +void Flightplan::setMinTransectLength(double minTransectLength) +{ + _minTransectLength = minTransectLength; +} + +Flightplan::ScenarioCPtr Flightplan::scenario() const +{ + return _scenario; +} + +void Flightplan::setScenario(ScenarioCPtr &scenario) +{ + _scenario = scenario; } -bool waypoints(const BoostPolygon &mArea, - const BoostPolygon &jArea, - std::vector &tiles, - std::vector &progress, - BoostPoint &home, - double lineDistance, - double minTransectLength, - std::vector, - size_t arrivalPathLength, - size_t returnPathLength) +Flightplan::ProgressCPtr Flightplan::progress() const { + return _progress; +} + +void Flightplan::setProgress(ProgressCPtr &progress) +{ + _progress = progress; +} + +struct Flightplan::RoutingDataModel{ + Matrix distanceMatrix; + long numVehicles; + RoutingIndexManager::NodeIndex depot; +}; + +bool Flightplan::update() +{ + _waypoints.clear(); + _arrivalPath.clear(); + _returnPath.clear(); + #ifdef SHOW_TIME auto start = std::chrono::high_resolution_clock::now(); #endif - if (!_generateTransects(mArea, lineDistance, minTransectLength, transects)) + if (!_generateTransects()) return false; #ifdef SHOW_TIME auto delta = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start); @@ -577,20 +798,21 @@ bool waypoints(const BoostPolygon &mArea, // Route Transects using Google or-tools. //======================================= // Offset joined area. + const BoostPolygon &jArea = _scenario->joinedArea(); BoostPolygon jAreaOffset; offsetPolygon(jArea, jAreaOffset, detail::offsetConstant); // Create vertex list; BoostLineString vertices; - size_t n_t = transects.size()*2; + size_t n_t = _transects.size()*2; size_t n0 = n_t+1; vertices.reserve(n0); - for (auto lstring : transects){ - for (auto vertex : lstring){ + for (auto& lstring : _transects){ + for (auto& vertex : lstring){ vertices.push_back(vertex); } } - vertices.push_back(home); + vertices.push_back(_scenario->homePositon()); for (long i=0; iValue(routing.NextVar(index)); route.push_back(manager.IndexToNode(index).value()); } + long sz = route.size(); + + // Helper Lambda. + auto fromVertices = [&vertices](const std::vector &idxArray, + std::vector &path){ + for (size_t j=1; j pathIdx; + shortestPathFromGraph(connectionGraph, idx0, idx1, pathIdx); + fromVertices(pathIdx, _arrivalPath); + + if (_arrivalPath.size() < 2) + return false; - // Connect transects - waypoint.push_back(vertices[route[0]]); - vector pathIdx; - arrivalPathLength = 0; - for (long i=0; i &tiles, - const std::vector &progress, - vector &transects, - std::string &errorString) +bool Flightplan::_generateTransects() { - if (tiles.size() != progress.size()){ + _transects.clear(); + if (_scenario->tiles().size() != _progress->size()){ ostringstream strstream; strstream << "Number of tiles (" - << _scenario.getTilesENU().size() + << _scenario->tiles().size() << ") is not equal to progress array length (" - << _progress.size() + << _progress->size() << ")"; errorString = strstream.str(); return false; } // Calculate processed tiles (_progress[i] == 100) and subtract them from measurement area. - size_t num_tiles = progress.size(); + size_t num_tiles = _progress->size(); vector processedTiles; { - for (size_t i = 0; i < num_tiles; ++i) { - if (progress[i] == 100){ + const auto &tiles = _scenario->tiles(); + for (size_t i=0; i(vertex.get<0>()*CLIPPER_SCALE), - static_cast(vertex.get<1>()*CLIPPER_SCALE) - } - ); + for ( auto vertex : _scenario->measurementArea().outer() ){ + mAreaClipper.push_back(ClipperLib::IntPoint{static_cast(vertex.get<0>()*CLIPPER_SCALE), + static_cast(vertex.get<1>()*CLIPPER_SCALE)}); } vector processedTilesClipper; for (auto t : processedTiles){ @@ -761,8 +1004,7 @@ bool FlightPlan::_generateTransects(double lineDistance, processedTilesClipper.push_back(path); } - const BoundingBox bbox; - minimalBoundingBox(mArea, bbox); + const min_bbox_rt &bbox = _scenario->mAreaBoundingBox(); double alpha = bbox.angle; double x0 = bbox.corners.outer()[0].get<0>(); double y0 = bbox.corners.outer()[0].get<1>(); @@ -770,18 +1012,18 @@ bool FlightPlan::_generateTransects(double lineDistance, double bboxHeight = bbox.height; double delta = detail::offsetConstant; - size_t num_t = int(ceil((bboxHeight + 2*delta)/lineDistance)); // number of transects + size_t num_t = int(ceil((bboxHeight + 2*delta)/_lineDistance)); // number of transects vector yCoords; yCoords.reserve(num_t); double y = -delta; for (size_t i=0; i < num_t; ++i) { yCoords.push_back(y); - y += lineDistance; + y += _lineDistance; } // Generate transects and convert them to clipper path. - trans::rotate_transformer rotate_back(-alpha*180/M_PI); + trans::rotate_transformer rotate_back(-alpha*180/M_PI); trans::translate_transformer translate_back(x0, y0); vector transectsClipper; transectsClipper.reserve(num_t); @@ -833,15 +1075,61 @@ bool FlightPlan::_generateTransects(double lineDistance, static_cast(clipperTransect[1].Y)/CLIPPER_SCALE}; BoostLineString transect{v1, v2}; - if (bg::length(transect) >= minTransectLength) - transects.push_back(transect); + if (bg::length(transect) >= _minTransectLength) + _transects.push_back(transect); } - if (transects.size() < 1) + if (_transects.size() < 1) return false; return true; } +void Flightplan::_generateRoutingModel(const BoostLineString &vertices, + const BoostPolygon &polygonOffset, + size_t n0, + Flightplan::RoutingDataModel &dataModel, + Matrix &graph) +{ +#ifdef SHOW_TIME + auto start = std::chrono::high_resolution_clock::now(); +#endif + graphFromPolygon(polygonOffset, vertices, graph); +#ifdef SHOW_TIME + auto delta = std::chrono::duration_cast(std::chrono::high_resolution_clock::now()-start); + cout << "Execution time graphFromPolygon(): " << delta.count() << " ms" << endl; +#endif +// cout << endl; +// for (size_t i=0; i &row = graph[i]; +// for (size_t j=0; j distanceMatrix(graph); +#ifdef SHOW_TIME + start = std::chrono::high_resolution_clock::now(); +#endif + toDistanceMatrix(distanceMatrix); +#ifdef SHOW_TIME + delta = std::chrono::duration_cast(std::chrono::high_resolution_clock::now()-start); + cout << "Execution time toDistanceMatrix(): " << delta.count() << " ms" << endl; +#endif + + dataModel.distanceMatrix.setDimension(n0, n0); + for (size_t i=0; i #include #include +#include #include "snake_typedefs.h" -#include "snake_geometry.h" #include #include @@ -81,12 +81,16 @@ private: std::vector _matrix; }; -typedef struct { +struct BoundingBox{ + BoundingBox(); + + void clear(); + double width; double height; double angle; BoostPolygon corners; -}BoundingBox; +}; template void toENU(const GeoPoint &origin, const GeoPoint &in, BoostPoint &out) @@ -144,8 +148,67 @@ void shortestPathFromGraph(const Matrix &graph, std::vector &pathIdx); //========================================================================= -// Tile calculation. +// Scenario. //========================================================================= +class Scenario{ + public: + Scenario(); + + void setMeasurementArea (const BoostPolygon &area); + void setServiceArea (const BoostPolygon &area); + void setCorridor (const BoostPolygon &area); + + BoostPolygon &measurementArea(); + BoostPolygon &serviceArea(); + BoostPolygon &corridor(); + + const BoundingBox &mAreaBoundingBox() const; + + const BoostPolygon &measurementArea() const; + const BoostPolygon &serviceArea()const; + const BoostPolygon &corridor()const; + + double tileWidth() const; + void setTileWidth(double tileWidth); + + double tileHeight() const; + void setTileHeight(double tileHeight); + + double minTileArea() const; + void setMinTileArea(double minTileArea); + + const BoostPolygon &joinedArea() const; + const vector &tiles() const; + const BoostLineString &tileCenterPoints() const; + const BoundingBox &measurementAreaBBox() const; + const BoostPoint &homePositon() const; + + bool update(); + + string errorString; + +private: + bool _calculateBoundingBox(); + bool _calculateTiles(); + bool _calculateJoinedArea(); + + double _tileWidth; + double _tileHeight; + double _minTileArea; + + mutable bool _needsUpdate; + + BoostPolygon _mArea; + BoostPolygon _sArea; + BoostPolygon _corridor; + BoostPolygon _jArea; + + BoundingBox _mAreaBoundingBox; + vector _tiles; + BoostLineString _tileCenterPoints; + BoostPoint _homePosition; +}; + template class Container> void areaToEnu( const GeoPoint &origin, const Container &in, @@ -156,7 +219,7 @@ void areaToEnu( const GeoPoint &origin, toENU(origin, vertex, p); out.outer().push_back(p); } - bg::correct(_measurementAreaENU); + bg::correct(out); } template class Container> @@ -171,24 +234,61 @@ void areaFromEnu( const GeoPoint &origin, } } -bool calculateTiles(const BoostPolygon &mArea, - double tileWidth, - double tileHeight, - double minTileArea, - std::vector &tiles, - std::string &errorString); bool joinAreas(const std::vector &areas, BoostPolygon &joinedArea); +//======================================================================================== +// Flightplan +//======================================================================================== -//========================================================================= -// Waypoint calculation. -//========================================================================= -bool waypoints(const BoostPolygon &mArea, const BoostPolygon &jArea, - std::vector &tiles, - BoostPoint &home, - double lineDistance, - double minTransectLength, std::vector, size_t arrivalPathLength, size_t returnPathLength); +class Flightplan{ +public: + using ScenarioPtr = shared_ptr; + using ProgressPtr = shared_ptr>; + + Flightplan(ScenarioPtr s, ScenarioPtr p); + + const vector &waypoints(void) const {return _waypoints;} + const vector &arrivalPath(void) const {return _arrivalPath;} + const vector &returnPath(void) const {return _returnPath;} + + double lineDistance() const; + void setLineDistance(double lineDistance); + + double minTransectLengthconst; + void setMinTransectLength(double minTransectLength); + + ScenarioPtr scenario() const; + void setScenario(ScenarioPtr &scenario); + + ProgressPtr progress() const; + void setProgress(ProgressPtr &progress); + + bool update(); + + string errorString; + +private: + // Search Filter to speed up routing.SolveWithParameters(...); + // Found here: http://www.lia.disi.unibo.it/Staff/MicheleLombardi/or-tools-doc/user_manual/manual/ls/ls_filtering.html + class SearchFilter; + struct RoutingDataModel; + + bool _generateTransects(); + void _generateRoutingModel(const BoostLineString &vertices, + const BoostPolygon &polygonOffset, + size_t n0, + RoutingDataModel &dataModel, + Matrix &graph); + double _lineDistance; + double _minTransectLength; + shared_ptr _pScenario; + shared_ptr> _pProgress; + vector _waypoints; + vector _arrivalPath; + vector _returnPath; + vector _transects; +}; namespace detail { const double offsetConstant = 0.1; // meter, polygon offset to compenstate for numerical inaccurracies. diff --git a/src/Snake/snake_geometry.cpp b/src/Snake/snake_geometry.cpp deleted file mode 100644 index e02abfc9b0..0000000000 --- a/src/Snake/snake_geometry.cpp +++ /dev/null @@ -1 +0,0 @@ - diff --git a/src/Snake/snake_geometry.h b/src/Snake/snake_geometry.h deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/src/Wima/Snake/SnakeWorker.cc b/src/Wima/Snake/SnakeWorker.cc index 4aa8dd3a68..767942a398 100644 --- a/src/Wima/Snake/SnakeWorker.cc +++ b/src/Wima/Snake/SnakeWorker.cc @@ -1,208 +1,273 @@ #include "SnakeWorker.h" #include +#include -#include "snake_geometry.h" #include "snake.h" +SnakeDataManager::SnakeDataManager(QObject *parent) + : QThread(parent) + , _pScenario(std::make_shared()) + , _pProgress(std::make_shared>()) + , _pFlightplan(std::make_shared(_pScenario, _pProgress)) + , _pData(std::make_shared()) -using namespace snake; -using namespace snake_geometry; - -SnakeWorker::SnakeWorker(QObject *parent) : QThread(parent) - , _lineDistance(3) - , _minTransectLength(2) - , _tileWidth(5) - , _tileHeight(5) - , _minTileArea(3) { } -SnakeWorker::~SnakeWorker() +SnakeDataManager::~SnakeDataManager() { } -WorkerResult_t &SnakeWorker::result() +SnakeDataManager::SnakeDataPtr SnakeDataManager::snakeData() { - return _result; + return _pData; } -bool SnakeWorker::precondition() const +bool SnakeDataManager::precondition() const { return _mArea.size() > 0 && _sArea.size() > 0 && _corridor.size() > 0 - && _progress.size() > 0; + && _pProgress->size() > 0; } -double SnakeWorker::lineDistance() const +double SnakeDataManager::lineDistance() const { - return _lineDistance; + _pFlightplan->lineDistance(); } -void SnakeWorker::run() +void SnakeDataManager::run() { - // Reset _result struct. - _result.clear(); + { + QMutexLocker lk(&_pData->m); + _pData->clear(); + } - if ( !precondition()) + if ( !precondition() ) return; - Scenario scenario;// Initialize scenario. - - Area mArea; - for (auto vertex : _mArea){ - mArea.geoPolygon.push_back(GeoPoint2D{vertex.latitude(), vertex.longitude()}); + if ( _mArea.size() <= 0 + || _sArea.size() <= 0 + || _corridor.size() <= 0 ){ + QMutexLocker lk(&_pData->m); + _pData->errorMessage = "At least one area invald. Size < 1."; } - mArea.type = AreaType::MeasurementArea; - Area sArea; - for (auto vertex : _sArea){ - sArea.geoPolygon.push_back(GeoPoint2D{vertex.latitude(), vertex.longitude()}); + auto origin = _mArea.front(); + // Measurement area update necessary. + if ( _mArea.size() != _pScenario->measurementArea().outer().size() ){ + { + QMutexLocker lk(&_pData->m); + _pData->ENUorigin = origin; + } + for (auto geoVertex : _mArea){ + snake::BoostPoint p; + snake::toENU(origin, geoVertex, p); + _pScenario->measurementArea().outer().push_back(p); + } } - sArea.type = AreaType::ServiceArea; - Area corridor; - for (auto vertex : _corridor){ - corridor.geoPolygon.push_back(GeoPoint2D{vertex.latitude(), vertex.longitude()}); + // Service area update necessary. + if ( _sArea.size() != _pScenario->serviceArea().outer().size() ){ + for (auto geoVertex : _sArea){ + snake::BoostPoint p; + snake::toENU(origin, geoVertex, p); + _pScenario->serviceArea().outer().push_back(p); + } } - corridor.type = AreaType::Corridor; - scenario.addArea(mArea); - scenario.addArea(sArea); - scenario.addArea(corridor); + // Service area update necessary. + if ( _corridor.size() != _pScenario->corridor().outer().size() ){ + for (auto geoVertex : _corridor){ + snake::BoostPoint p; + snake::toENU(origin, geoVertex, p); + _pScenario->corridor().outer().push_back(p); + } + } - if ( !scenario.update(_tileWidth, _tileHeight, _minTileArea) ) + if ( !_pScenario->update() ){ + { + QMutexLocker lk(&_pData->m); + for (auto c : _pScenario->errorString){ + _pData->errorMessage.push_back(QChar(c)); + } + } return; + } - FlightPlan flightplan; - flightplan.setScenario(scenario); - flightplan.setProgress(_progress); + // Asynchronously update flightplan. + auto future = std::async(std::bind(&snake::Flightplan::update, _pFlightplan)); + // Continue with storing scenario data in the mean time. + { + QMutexLocker lk(&_pData->m); + // Get tiles. + const auto &tiles = _pScenario->tiles(); + const auto ¢erPoints = _pScenario->tileCenterPoints(); + for ( unsigned int i=0; i < tiles.size(); ++i ) { + const auto &tile = tiles[i]; + GeoTile geoTile; + Polygon2D 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>(), 0.0); + QGeoCoordinate geoVertex; + snake::fromENU(origin, *it, geoVertex); + enuTile.polygon().push_back(enuVertex); + geoTile.push_back(QVertex); + } + const auto &boostPoint = centerPoints[i]; + QPointF enuVertex(boostPoint.get<0>(), boostPoint.get<1>(), 0.0); + QGeoCoordinate geoVertex; + snake::fromENU(origin, enuVertex, geoVertex); + geoTile.setCenter(geoVertex); + _pData->tiles.polygons().push_back(geoTile); + _pData->tileCenterPoints.push_back(QVariant::fromValue(geoVertex)); + _pData->tilesENU.polygons().push_back(enuTile); + _pData->tileCenterPointsENU.push_back(enuVertex); + } + } + + future.wait(); // Trying to generate flight plan. - if ( !flightplan.generate(_lineDistance, _minTransectLength) ){ + if ( !future.get() ){ // error - for (auto c : flightplan.errorString){ - _result.errorMessage.push_back(QChar(c)); + QMutexLocker lk(&_pData->m); + for (auto c : _pFlightplan->errorString){ + _pData->errorMessage.push_back(QChar(c)); } } else { //success!!! - _result.success = true; - - // Fill result struct. - auto& waypoints = flightplan.getWaypoints(); - for (auto vertex : waypoints){ - _result.waypoints.append(QGeoCoordinate(vertex[0], vertex[1], 0)); + QMutexLocker lk(&_pData->m); + // Store waypoints. + for (auto boostVertex : _pFlightplan->waypoints()){ + QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>(), 0.0}; + QGeoCoordinate geoVertex; + snake::fromENU(origin, enuVertex, geoVertex); + _pData->waypointsENU.push_back(enuVertex); + _pData->waypoints.push_back(geoVertex); } - - auto& arrivalPathIdx = flightplan.getArrivalPathIdx(); - for (auto idx : arrivalPathIdx){ - _result.arrivalPathIdx.append(idx); + // Store arrival path. + for (auto boostVertex : _pFlightplan->arrivalPath()){ + QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>(), 0.0}; + QGeoCoordinate geoVertex; + snake::fromENU(origin, enuVertex, geoVertex); + _pData->arrivalPathENU.push_back(enuVertex); + _pData->arrivalPath.push_back(geoVertex); } - - auto& returnPathIdx = flightplan.getReturnPathIdx(); - for (auto idx : returnPathIdx){ - _result.returnPathIdx.append(idx); + // Store return path. + for (auto boostVertex : _pFlightplan->returnPath()){ + QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>(), 0.0}; + QGeoCoordinate geoVertex; + snake::fromENU(origin, enuVertex, geoVertex); + _pData->returnPathENU.push_back(enuVertex); + _pData->returnPath.push_back(geoVertex); } - - - // Get tiles and origin. - auto origin = scenario.getOrigin(); - _result.origin.setLatitude(origin[0]); - _result.origin.setLongitude(origin[1]); - _result.origin.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]; - GeoTile geoTile; - for ( const auto &vertex : tile) { - QGeoCoordinate QVertex(vertex[0], vertex[1], vertex[2]); - geoTile.append(QVertex); - } - const auto ¢erPoint = cps[i]; - QGeoCoordinate c(centerPoint[0], centerPoint[1], centerPoint[2]); - geoTile.setCenter(c); - _result.tiles.polygons().append(geoTile); - _result.tileCenterPoints.append(QVariant::fromValue(c)); - } - - // Get local tiles. - const auto &tilesENU = scenario.getTilesENU(); - for ( unsigned int i=0; i < tilesENU.size(); ++i ) { - const auto &tile = tilesENU[i]; - Polygon2D localTile; - for ( const auto &vertex : tile.outer()) { - QPointF QVertex(vertex.get<0>(), vertex.get<1>()); - localTile.path().append(QVertex); - } - _result.tilesLocal.polygons().append(localTile); - } - } } -double SnakeWorker::minTransectLength() const +double SnakeDataManager::minTransectLength() const { - return _minTransectLength; + return _pFlightplan->minTransectLength(); } -void SnakeWorker::setMinTransectLength(double minTransectLength) +void SnakeDataManager::setMinTransectLength(double minTransectLength) { - if (minTransectLength > 0) - _minTransectLength = minTransectLength; + _pFlightplan->setMinTransectLength(minTransectLength); } -double SnakeWorker::minTileArea() const +double SnakeDataManager::minTileArea() const { - return _minTileArea; + return _pScenario->minTileArea(); } -void SnakeWorker::setLineDistance(double lineDistance) +void SnakeDataManager::setLineDistance(double lineDistance) { - if (lineDistance > 0) - _lineDistance = lineDistance; + _pFlightplan->setLineDistance(lineDistance); } -double SnakeWorker::tileWidth() const +double SnakeDataManager::tileWidth() const { - return _tileWidth; + return _pScenario->tileWidth(); } -void SnakeWorker::setTileWidth(double tileWidth) +void SnakeDataManager::setTileWidth(double tileWidth) { - if (tileWidth > 0) - _tileWidth = tileWidth; + _pScenario->setTileWidth(tileWidth); } -double SnakeWorker::tileHeight() const +double SnakeDataManager::tileHeight() const { - return _tileHeight; + return _pScenario->_tileHeight(); } -void SnakeWorker::setTileHeight(double tileHeight) +void SnakeDataManager::setTileHeight(double tileHeight) { - if (tileHeight > 0) - _tileHeight = tileHeight; + _pScenario->setTileHeight(tileHeight); } -void SnakeWorker::setMinTileArea(double minTileArea) +void SnakeDataManager::setMinTileArea(double minTileArea) { - if (minTileArea > 0) - _minTileArea = minTileArea; + _pScenario->setMinTileArea(minTileArea); } -void Result::clear() +SnakeData::SnakeData() +{ + +} + +void SnakeData::clear() { - this->success = false; - this->errorMessage = ""; this->waypoints.clear(); - this->returnPathIdx.clear(); - this->arrivalPathIdx.clear(); + this->arrivalPath.clear(); + this->returnPath.clear(); this->tiles.polygons().clear(); this->tilesLocal.polygons().clear(); this->tileCenterPoints.clear(); + this->waypointsENU.clear(); + this->arrivalPathENU.clear(); + this->returnPathENU.clear(); + this->tilesENU.polygons().clear(); + this->tileCenterPoints.clear(); + this->errorMessage = ""; +} + +void SnakeDataManager::setCorridor(const QList &corridor) +{ + _corridor.clear(); + _pScenario->corridor().clear(); + for (auto vertex : corridor) { + _corridor.push_back(vertex); + } +} + +void SnakeDataManager::setProgress(const QList &progress) +{ + _pProgress->clear(); + for (auto p : progress) { + assert(p >= -1 && p <= 100); + _pProgress->push_back(p); + } +} + +void SnakeDataManager::setServiceArea(const QList &serviceArea) +{ + _sArea.clear(); + _pScenario->serviceArea().clear(); + for (auto vertex : serviceArea) { + _sArea.push_back(vertex); + } +} + +void SnakeDataManager::setMeasurementArea(const QList &measurementArea) +{ + _mArea.clear(); + _pScenario->measurementArea().clear(); + for (auto vertex : measurementArea) { + _mArea.push_back(vertex); + } } diff --git a/src/Wima/Snake/SnakeWorker.h b/src/Wima/Snake/SnakeWorker.h index 4ea77f761e..9377815423 100644 --- a/src/Wima/Snake/SnakeWorker.h +++ b/src/Wima/Snake/SnakeWorker.h @@ -5,46 +5,63 @@ #include #include #include +#include + #include +#include #include "GeoPolygonArray.h" #include "PolygonArray.h" +#include "SnakeWorker.h" +#include "QNemoProgress.h" typedef QList QVariantList; -typedef struct Result{ +namespace snake { + class Scenario; + class Flightplan; +} + +struct SnakeData{ + SnakeData(); + QVector waypoints; + QVector arrivalPath; + QVector returnPath; + QGeoCoordinate ENUorigin; + QVector waypointsENU; + QVector arrivalPathENU; + QVector returnPathENU; GeoPolygonArray tiles; - PolygonArray tilesLocal; QVariantList tileCenterPoints; - QGeoCoordinate origin; - QVector arrivalPathIdx; - QVector returnPathIdx; - bool success; + PolygonArray tilesENU; + QVector tileCenterPointsENU; QString errorMessage; + mutable QMutex m; void clear(); -}WorkerResult_t; +}; -class SnakeWorker : public QThread{ +class SnakeDataManager : public QThread{ Q_OBJECT public: - SnakeWorker(QObject *parent = nullptr); - ~SnakeWorker() override; + using ProgressPtr = std::shared_ptr>; + using ScenarioPtr = std::shared_ptr; + using FlightPlanPtr = std::shared_ptr; + using SnakeDataPtr = std::shared_ptr; + + SnakeDataManager(QObject *parent = nullptr); + ~SnakeDataManager() override; - template class Container> - void setMeasurementArea (const Container &measurementArea); - template class Container> - void setServiceArea (const Container &serviceArea); - template class Container> - void setCorridor (const Container &corridor); - template class Container, class IntType> - void setProgress (const Container &progress); - WorkerResult_t &result(); + void setMeasurementArea (const QList &measurementArea); + void setServiceArea (const QList &serviceArea); + void setCorridor (const QList &corridor); + + SnakeDataPtr snakeData(); double lineDistance() const; void setLineDistance(double lineDistance); @@ -67,53 +84,20 @@ protected: private: bool precondition() const; - std::vector _mArea; std::vector _sArea; std::vector _corridor; - std::vector _progress; - double _lineDistance; - double _minTransectLength; - double _tileWidth; - double _tileHeight; - double _minTileArea; - WorkerResult_t _result; + ScenarioPtr _pScenario; + ProgressPtr _pProgress; + FlightPlanPtr _pFlightplan; + SnakeDataPtr _pData; }; -template class Container> -void SnakeWorker::setCorridor(const Container &corridor) -{ - _corridor.clear(); - for (auto vertex : corridor) { - _corridor.push_back(vertex); - } -} -template class Container> -void SnakeWorker::setMeasurementArea(const Container &measurementArea) -{ - _mArea.clear(); - for (auto vertex : measurementArea) { - _mArea.push_back(vertex); - } -} -template class Container> -void SnakeWorker::setServiceArea(const Container &serviceArea) -{ - _sArea.clear(); - for (auto vertex : serviceArea) { - _sArea.push_back(vertex); - } -} -template class Container, class IntType> -void SnakeWorker::setProgress(const Container &progress) -{ - _progress.clear(); - for (auto p : progress) { - assert(p >= -1 && p <= 100); - _progress.push_back(p); - } -} + + + + diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index e61f64c89a..24bf234c53 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -112,9 +112,9 @@ WimaController::WimaController(QObject *parent) _eventTimer.start(EVENT_TIMER_INTERVAL); // Snake Worker Thread. - connect(&_snakeWorker, &SnakeWorker::finished, this, &WimaController::_updateSnakeData); + connect(&_snakeWorker, &SnakeDataManager::finished, this, &WimaController::_updateSnakeData); connect(this, &WimaController::nemoProgressChanged, this, &WimaController::_initStartSnakeWorker); - connect(this, &QObject::destroyed, &this->_snakeWorker, &SnakeWorker::quit); + connect(this, &QObject::destroyed, &this->_snakeWorker, &SnakeDataManager::quit); // Snake. connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_switchSnakeManager); diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index 1ae1ac0f50..c2e49dcfad 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -401,7 +401,7 @@ private: // Snake bool _snakeCalcInProgress; - SnakeWorker _snakeWorker; + SnakeDataManager _snakeWorker; GeoPoint _snakeOrigin; GeoPolygonArray _snakeTiles; // tiles PolygonArray _snakeTilesLocal; // tiles local coordinate system -- GitLab