diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 2b11cb8b0e56a43351d472b682eef694aad79c08..622684a8e69aca8534b135cebbbd0daed99d0872 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -408,6 +408,7 @@ 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,22 +431,22 @@ HEADERS += \ src/Snake/mapbox/variant_io.hpp \ src/Snake/snake.h \ src/Snake/snake_geometry.h \ - src/Snake/snake_global.h \ + src/Snake/snake_typedefs.h \ src/Wima/Geometry/GeoPoint3D.h \ src/Wima/Geometry/Polygon2D.h \ src/Wima/Geometry/PolygonArray.h \ + src/Wima/Snake/GeoPolygonArray.h \ + src/Wima/Snake/GeoTile.h \ + src/Wima/Snake/PolygonArray.h \ src/Wima/Snake/QNemoHeartbeat.h \ src/Wima/Snake/QNemoProgress.h \ src/Wima/Snake/QtROSJsonFactory.h \ src/Wima/Snake/QtROSTypeFactory.h \ - src/Wima/Snake/SnakeTiles.h \ - src/Wima/Snake/SnakeTilesLocal.h \ src/Wima/Snake/SnakeWorker.h \ src/Wima/WaypointManager/AreaInterface.h \ src/Wima/WaypointManager/DefaultManager.h \ src/Wima/WaypointManager/GenericWaypointManager.h \ src/Wima/Geometry/WimaPolygonArray.h \ - src/Wima/Snake/snaketile.h \ src/Wima/WaypointManager/RTLManager.h \ src/Wima/WaypointManager/Settings.h \ src/Wima/WaypointManager/Slicer.h \ @@ -497,10 +498,12 @@ 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 \ src/Wima/Snake/QNemoProgress.cc \ src/Wima/Snake/SnakeWorker.cc \ src/Wima/WaypointManager/AreaInterface.cpp \ @@ -516,7 +519,6 @@ SOURCES += \ src/comm/ros_bridge/include/TopicPublisher.cpp \ src/comm/ros_bridge/include/TopicSubscriber.cpp \ src/comm/ros_bridge/src/CasePacker.cpp \ - src/Wima/Snake/snaketile.cpp \ src/api/QGCCorePlugin.cc \ src/api/QGCOptions.cc \ src/api/QGCSettings.cc \ diff --git a/src/Snake/flight_plan.cpp b/src/Snake/flight_plan.cpp new file mode 100644 index 0000000000000000000000000000000000000000..ae9aa47e1331f9c6205714a2a29325d130993bdc --- /dev/null +++ b/src/Snake/flight_plan.cpp @@ -0,0 +1,372 @@ +#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 1b079432fe68c2c7cb4a08cc20b64103c5c9ebc9..1098ea3d2c869668deb3e81a0b31d01d844c7bd4 100644 --- a/src/Snake/snake.cpp +++ b/src/Snake/snake.cpp @@ -1,13 +1,13 @@ #include #include "snake.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" +#include +#include + +#include +#include +#include using namespace operations_research; @@ -15,165 +15,453 @@ using namespace operations_research; //#define SHOW_TIME #endif -using namespace snake_geometry; -using namespace std; - namespace bg = boost::geometry; namespace trans = bg::strategy::transform; namespace snake { +//========================================================================= +// Geometry stuff. +//========================================================================= + +BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian) -Scenario::Scenario() : - _mAreaBoundingBox(min_bbox_rt{0, 0, 0, BoostPolygon{}}) +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>()); + lr1.push_back(vertex); +} +p.push_back(lr1); +geometry::point c = polylabel(p); +center.set<0>(c.x); +center.set<1>(c.y); } -bool Scenario::addArea(Area &area) - { - if (area.geoPolygon.size() < 3){ - errorString = "Area has less than three vertices."; - return false; - } - if (area.type == MeasurementArea) - return Scenario::_setMeasurementArea(area); - else if (area.type == ServiceArea) - return Scenario::_setServiceArea(area); - else if (area.type == Corridor) - return Scenario::_setCorridor(area); - return false; +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; } -bool Scenario::update(double tileWidth, double tileHeight, double minTileArea) +void toBoost(const Point2D &point, BoostPoint &boost_point) { - if (!_areas2enu()) - return false; - if (!_calculateBoundingBox()) - return false; - if (!_calculateTiles(tileWidth, tileHeight, minTileArea)) - return false; - if (!_calculateJoinedArea()) - return false; + boost_point.set<0>(point[0]); + boost_point.set<1>(point[1]); +} - return true; +void fromBoost(const BoostPoint &boost_point, Point2D &point) +{ + point[0] = boost_point.get<0>(); + point[1] = boost_point.get<1>(); } -bool Scenario::_areas2enu() +void toBoost(const Point2DList &point_list, BoostPolygon &boost_polygon) { - if (_measurementArea.geoPolygon.size() > 0){ - _measurementAreaENU.clear(); - for(auto vertex : _measurementArea.geoPolygon) { - Point3D ENUVertex; - toENU(_geoOrigin, Point3D{vertex[0], vertex[1], _measurementArea.altitude}, ENUVertex); - _measurementAreaENU.outer().push_back(BoostPoint{ENUVertex[0], ENUVertex[1]}); - } - bg::correct(_measurementAreaENU); - - _serviceAreaENU.clear(); - if (_serviceArea.geoPolygon.size() > 0){ - for(auto vertex : _serviceArea.geoPolygon) { - Point3D ENUVertex; - toENU(_geoOrigin, Point3D{vertex[0], vertex[1], _serviceArea.altitude}, ENUVertex); - _serviceAreaENU.outer().push_back(BoostPoint{ENUVertex[0], ENUVertex[1]}); - } - } else{ - errorString = "Service area has no vertices."; - return false; - } - bg::correct(_serviceAreaENU); - polygonCenter(_serviceAreaENU, _homePositionENU); - fromENU(_geoOrigin, Point3D{_homePositionENU.get<0>(), _homePositionENU.get<1>(), 0}, _homePosition); - - _corridorENU.clear(); - if (_corridor.geoPolygon.size() > 0){ - for(auto vertex : _corridor.geoPolygon) { - Point3D ENUVertex; - toENU(_geoOrigin, Point3D{vertex[0], vertex[1], _corridor.altitude}, ENUVertex); - _corridorENU.outer().push_back(BoostPoint{ENUVertex[0], ENUVertex[1]}); - } - } - bg::correct(_corridorENU); + for (auto vertex : point_list) { + BoostPoint boost_vertex; + toBoost(vertex, boost_vertex); + boost_polygon.outer().push_back(boost_vertex); + } + bg::correct(boost_polygon); +} - return true; - } +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); + } +} - errorString = "Measurement area has no vertices."; - return false; +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); } -bool Scenario::_setMeasurementArea(Area &area) +void rotateRad(const Point2DList &point_list, Point2DList &rotated_point_list, double rad) { - if (area.geoPolygon.size() <= 0) - return false; - GeoPoint2D origin2D = area.geoPolygon[0]; - _geoOrigin = GeoPoint3D{origin2D[0], origin2D[1], 0}; - _measurementArea = area; - _measurementAreaENU.clear(); - _serviceAreaENU.clear(); - _corridorENU.clear(); - return true; + 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; } -bool Scenario::_setServiceArea(Area &area) +void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset, double offset) { - if (area.geoPolygon.size() <= 0) - return false; - _serviceArea = area; - _serviceAreaENU.clear(); - return true; + 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::buffer(polygon, result, distance_strategy, side_strategy, join_strategy, end_strategy, point_strategy); + + if (result.size() > 0) + polygonOffset = result[0]; + } -bool Scenario::_setCorridor(Area &area) + +void graphFromPolygon(const BoostPolygon &polygon, const BoostLineString &vertices, Matrix &graph) { - if (area.geoPolygon.size() <= 0) - return false; - _corridor = area; - _corridorENU.clear(); - return true; + 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}; + + 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 Scenario::_calculateBoundingBox() +bool dijkstraAlgorithm(const size_t numElements, + size_t startIndex, + size_t endIndex, + std::vector &elementPath, + std::function distanceDij) { - minimalBoundingBox(_measurementAreaENU, _mAreaBoundingBox); - return true; + 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; } +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); + } + } +} -/** - * 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 _measurementAreaENU 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(double tileWidth, double tileHeight, double minTileArea) +void shortestPathFromGraph(const Matrix &graph, size_t startIndex, size_t endIndex, std::vector &pathIdx) { - _tilesENU.clear(); - _tileCenterPointsENU.clear(); - _tiles.clear(); - _tileCenterPoints.clear(); - if (tileWidth <= 0 || tileHeight <= 0 || minTileArea <= 0) { + 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; + } + +} + +//========================================================================= +// Tile 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) { errorString = "Parameters tileWidth, tileHeight, minTileArea must be positive."; return false; } - double bbox_width = _mAreaBoundingBox.width; - double bbox_height = _mAreaBoundingBox.height; - - BoostPoint origin = _mAreaBoundingBox.corners.outer()[0]; + BoundingBox bbox; + minimalBoundingBox(mArea, bbox); + double bbox_width = bbox.width; + double bbox_height = bbox.height; + BoostPoint origin = bbox.corners.outer()[0]; //cout << "Origin: " << origin[0] << " " << origin[1] << endl; // Transform _measurementAreaENU polygon to bounding box coordinate system. - trans::rotate_transformer rotate(_mAreaBoundingBox.angle*180/M_PI); + trans::rotate_transformer rotate(bbox.angle*180/M_PI); trans::translate_transformer translate(-origin.get<0>(), -origin.get<1>()); BoostPolygon translated_polygon; BoostPolygon rotated_polygon; @@ -187,12 +475,11 @@ bool Scenario::_calculateTiles(double tileWidth, double tileHeight, double minTi size_t j_max = ceil(bbox_height/tileHeight); if (i_max < 1 || j_max < 1) { - errorString = "tileWidth or tileHeight to small."; + errorString = "tileWidth or tileHeight to large."; return false; } - - trans::rotate_transformer rotate_back(-_mAreaBoundingBox.angle*180/M_PI); + trans::rotate_transformer rotate_back(-bbox.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; @@ -223,118 +510,62 @@ bool Scenario::_calculateTiles(double tileWidth, double tileHeight, double minTi boost::geometry::transform(rotated_tile, translated_tile, translate_back); // Store tile and calculate center point. - _tilesENU.push_back(translated_tile); - BoostPoint tile_center; - polygonCenter(translated_tile, tile_center); - _tileCenterPointsENU.push_back(tile_center); + tiles.push_back(translated_tile); } } } } - if (_tilesENU.size() < 1){ + if (tiles.size() < 1){ errorString = "No tiles calculated. Is the minTileArea parameter large enough?"; return false; } - for ( auto tile : _tilesENU){ - GeoPoint3DList geoTile; - for ( int i=0; i < int(tile.outer().size())-1; ++i) { - BoostPoint vertex(tile.outer()[i]); - GeoPoint3D geoVertex; - fromENU(_geoOrigin, Point3D{vertex.get<0>(), vertex.get<1>(), 0}, geoVertex); - geoTile.push_back(geoVertex); - } - _tiles.push_back(geoTile); - } - - for ( auto vertex : _tileCenterPointsENU){ - GeoPoint3D geoVertex; - fromENU(_geoOrigin, Point3D{vertex.get<0>(), vertex.get<1>(), 0}, geoVertex); - _tileCenterPoints.push_back(geoVertex); - } - return true; } -bool Scenario::_calculateJoinedArea() +bool joinAreas(const std::vector &areas, BoostPolygon &joinedArea) { - _joinedAreaENU.clear(); - // Measurement area and service area overlapping? - bool overlapingSerMeas = bg::intersects(_measurementAreaENU, _serviceAreaENU) ? true : false; - bool corridorValid = _corridorENU.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(_corridorENU, _measurementAreaENU) ) { - // Corridor overlaping with service area? - if ( bg::intersects(_corridorENU, _serviceAreaENU) ) { - corridor_is_connection = true; - } - } - } + if (areas.size() < 1) + return false; + std::deque idxList; + for(size_t i = 1; i < areas.size(); ++i) + idxList.push_back(i); + BoostPolygon partialArea = areas[0]; - // Are areas joinable? std::deque sol; - BoostPolygon partialArea = _measurementAreaENU; - if (overlapingSerMeas){ - if(corridor_is_connection){ - bg::union_(partialArea, _corridorENU, sol); + while (idxList.size() > 0){ + bool success = false; + for (auto it = idxList.begin(); it != idxList.end(); ++it){ + bg::union_(partialArea, areas[*it], sol); + if (sol.size() > 0) { + partialArea = sol[0]; + sol.clear; + idxList.erase(it); + success = true; + break; + } } - } else if (corridor_is_connection){ - bg::union_(partialArea, _corridorENU, sol); - } else { - errorString = "Areas are not overlapping"; - return false; - } - - if (sol.size() > 0) { - partialArea = sol[0]; - sol.clear(); - } - - // Join areas. - bg::union_(partialArea, _serviceAreaENU, sol); - - if (sol.size() > 0) { - _joinedAreaENU = sol[0]; - } else { - return false; + if ( !success ) + return false; } - - return true; } - - -struct FlightPlan::RoutingDataModel{ - Matrix distanceMatrix; - long numVehicles; - RoutingIndexManager::NodeIndex depot; -}; - -FlightPlan::FlightPlan() +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) { - -} - -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)) + if (!_generateTransects(mArea, lineDistance, minTransectLength, transects)) return false; #ifdef SHOW_TIME auto delta = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start); @@ -346,21 +577,20 @@ bool FlightPlan::generate(double lineDistance, double minTransectLength) // 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 n_t = transects.size()*2; size_t n0 = n_t+1; vertices.reserve(n0); - for (auto lstring : _transects){ + for (auto lstring : transects){ for (auto vertex : lstring){ vertices.push_back(vertex); } } - vertices.push_back(_scenario.getHomePositonENU()); + vertices.push_back(home); for (long i=0; i pathIdx; - long arrivalPathLength = 0; + 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]}); + waypoints.push_back(vertices[pathIdx[j]]); } + returnPathLength = pathIdx.size(); return true; + } -bool FlightPlan::_generateTransects(double lineDistance, double minTransectLength) +bool FlightPlan::_generateTransects(double lineDistance, + double minTransectLength, + const BoostPolygon mArea, + const std::vector &tiles, + const std::vector &progress, + vector &transects, + std::string &errorString) { - _transects.clear(); - if (_scenario.getTilesENU().size() != _progress.size()){ + if (tiles.size() != progress.size()){ ostringstream strstream; strstream << "Number of tiles (" << _scenario.getTilesENU().size() @@ -512,12 +728,11 @@ bool FlightPlan::_generateTransects(double lineDistance, double minTransectLengt } // 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; { - const auto &tiles = _scenario.getTilesENU(); - for (size_t i=0; i(vertex.get<0>()*CLIPPER_SCALE), - static_cast(vertex.get<1>()*CLIPPER_SCALE)}); + for ( auto vertex : mArea.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){ @@ -542,7 +761,8 @@ bool FlightPlan::_generateTransects(double lineDistance, double minTransectLengt processedTilesClipper.push_back(path); } - const min_bbox_rt &bbox = _scenario.getMeasurementAreaBBoxENU(); + const BoundingBox bbox; + minimalBoundingBox(mArea, bbox); double alpha = bbox.angle; double x0 = bbox.corners.outer()[0].get<0>(); double y0 = bbox.corners.outer()[0].get<1>(); @@ -614,79 +834,14 @@ bool FlightPlan::_generateTransects(double lineDistance, double minTransectLengt BoostLineString transect{v1, v2}; if (bg::length(transect) >= minTransectLength) - _transects.push_back(transect); + 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 "snake_typedefs.h" #include "snake_geometry.h" +#include +#include + using namespace std; -using namespace snake_geometry; namespace snake { - enum AreaType {MeasurementArea, ServiceArea, Corridor}; +//========================================================================= +// Geometry stuff. +//========================================================================= +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;} - class Area { - public: - Area(); - Area(const GeoPoint2DList &gP, AreaType tp); - Area(const GeoPoint2DList &gP, double alt, size_t l, AreaType tp); + void set(size_t i, size_t j, const T &value) + { + assert(_isInitialized); + assert(i < _m); + assert(j < _n); + _matrix[i*_m+j] = value; + } - GeoPoint2DList geoPolygon; - double altitude; - size_t layers; - AreaType type; - }; + 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; +}; + +typedef struct { + double width; + double height; + double angle; + BoostPolygon corners; +}BoundingBox; + +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); +} + +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); + +//========================================================================= +// Tile calculation. +//========================================================================= +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(_measurementAreaENU); +} + +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); + } +} - //======================================================================================== - // Scenario - //======================================================================================== - class Scenario{ - public: - Scenario(); - - bool addArea(Area &area); - - const Area &getMeasurementArea() const {return _measurementArea;} - const Area &getServiceArea() const {return _serviceArea;} - const Area &getCorridor() const {return _corridor;} - - const BoostPolygon &getMeasurementAreaENU() {return _measurementAreaENU;} - const BoostPolygon &getServiceAreaENU() {return _serviceAreaENU;} - const BoostPolygon &getCorridorENU() {return _corridorENU;} - const BoostPolygon &getJoineAreaENU() {return _joinedAreaENU;} - const GeoPoint3D &getOrigin() {return _geoOrigin;} - - const vector &getTilesENU() {return _tilesENU;} - const BoostLineString &getTileCenterPointsENU() {return _tileCenterPointsENU;} - const min_bbox_rt &getMeasurementAreaBBoxENU() {return _mAreaBoundingBox;} - const BoostPoint &getHomePositonENU() {return _homePositionENU;} - - const vector &getTiles() {return _tiles;} - const vector &getTileCenterPoints() {return _tileCenterPoints;} - const GeoPoint3D &getHomePositon() {return _homePosition;} - - bool update(double tileWidth, double tileHeight, double minTileArea); - - string errorString; - - private: - bool _areas2enu(); - bool _setMeasurementArea(Area &area); - bool _setServiceArea(Area &area); - bool _setCorridor(Area &area); - bool _calculateBoundingBox(); - bool _calculateTiles(double tileWidth, double tileHeight, double minTileArea); - bool _calculateJoinedArea(); - - Area _measurementArea; - Area _serviceArea; - Area _corridor; - - BoostPolygon _measurementAreaENU; - BoostPolygon _serviceAreaENU; - BoostPolygon _corridorENU; - BoostPolygon _joinedAreaENU; - - min_bbox_rt _mAreaBoundingBox; - - vector _tilesENU; - BoostLineString _tileCenterPointsENU; - - vector _tiles; - vector _tileCenterPoints; - - GeoPoint3D _geoOrigin; - BoostPoint _homePositionENU; - - GeoPoint3D _homePosition; - }; - - //======================================================================================== - // FlightPlan - //======================================================================================== - - class FlightPlan{ - public: - FlightPlan(); - - void setScenario(Scenario &scenario) {_scenario = scenario;} - void setProgress(vector &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 - }; +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); + + +//========================================================================= +// 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); 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 index 60044db429496b658246d888f9ff255fb5ac5c0a..e02abfc9b0e17c18f0c365f044cc760d3b961f4a 100644 --- a/src/Snake/snake_geometry.cpp +++ b/src/Snake/snake_geometry.cpp @@ -1,452 +1 @@ -#include "snake_geometry.h" -#include -#include - -#include -#include -#include - - -#include -#include - -using namespace mapbox; -using namespace snake_geometry; -using namespace std; - -namespace bg = bg; -namespace trans = bg::strategy::transform; - -BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian) - - -namespace snake_geometry { - -void toENU(const GeoPoint3D &WGS84Reference, const GeoPoint3D &WGS84Position, Point3D &ENUPosition) -{ - GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f()); - GeographicLib::LocalCartesian proj(WGS84Reference[0], WGS84Reference[1], WGS84Reference[2], earth); - - proj.Forward(WGS84Position[0], WGS84Position[1], WGS84Position[2], ENUPosition[0], ENUPosition[1], ENUPosition[2]); -} - -void fromENU(const Point3D &WGS84Reference, const Point3D &CartesianPosition, GeoPoint3D &GeoPosition) -{ - GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f()); - GeographicLib::LocalCartesian proj(WGS84Reference[0], WGS84Reference[1], WGS84Reference[2], earth); - - proj.Reverse(CartesianPosition[0], CartesianPosition[1], CartesianPosition[2], GeoPosition[0], GeoPosition[1], GeoPosition[2]); -} - -void polygonCenter(const BoostPolygon &polygon, BoostPoint ¢er) -{ - 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>()); - lr1.push_back(vertex); - } - p.push_back(lr1); - geometry::point c = polylabel(p); - - center.set<0>(c.x); - center.set<1>(c.y); -} - -void minimalBoundingBox(const BoostPolygon &polygon, min_bbox_rt &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 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); - 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::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(); - - 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); - } - } - -} - -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; -} - -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; - } - -} - -} // end namespace snake_geometry + diff --git a/src/Snake/snake_geometry.h b/src/Snake/snake_geometry.h index c3592d522872170596d1c934d9d6be0b8f99c1a6..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 100644 --- a/src/Snake/snake_geometry.h +++ b/src/Snake/snake_geometry.h @@ -1,128 +0,0 @@ -#pragma once -#include -#include -#include -#include - -namespace bg = boost::geometry; - -namespace snake_geometry { - -typedef std::array Point2D; -typedef std::array Point3D; -typedef std::array GeoPoint3D; -typedef std::array GeoPoint2D; -typedef std::vector Point2DList; -typedef std::vector Point3DList; -typedef std::vector GeoPoint2DList; -typedef std::vector GeoPoint3DList; - -typedef bg::model::point BoostPoint; -//typedef std::vector BoostPointList; -typedef bg::model::polygon BoostPolygon; -typedef bg::model::linestring BoostLineString; -typedef std::vector> Int64Matrix; - - -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}); - } - -private: - size_t _elements; - size_t _m; - size_t _n; - bool _isInitialized; - std::vector _matrix; -}; - -typedef struct { - double width; - double height; - double angle; - BoostPolygon corners; -}min_bbox_rt; - -void toENU(const GeoPoint3D &WGS84Reference, const GeoPoint3D &WGS84Position, Point3D &ENUPosition); -void fromENU(const Point3D &WGS84Reference, const Point3D &ENUPosition, GeoPoint3D &WGS84Position); - -void polygonCenter(const BoostPolygon &polygon, BoostPoint ¢er); -void minimalBoundingBox(const BoostPolygon &polygon, min_bbox_rt &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); - - -void rotateDeg(const Point2DList &point_list, Point2DList &rotated_point_list, double degree); -void rotateRad(const Point2DList &point_list, Point2DList &rotated_point_list, double rad); - -bool isClockwise(const Point2DList &point_list); - -void toBoost(const Point2D &point, BoostPoint &boost_point); -void toBoost(const Point2DList &point_list, BoostPolygon &boost_polygon); - -void fromBoost(const BoostPoint &boost_point, Point2D &point); -void fromBoost(const BoostPolygon &boost_polygon, Point2DList &point_list); -} diff --git a/src/Snake/snake_global.h b/src/Snake/snake_global.h deleted file mode 100644 index 2cfda3daa0cf6fca8ddce877b3a5690cd1873296..0000000000000000000000000000000000000000 --- a/src/Snake/snake_global.h +++ /dev/null @@ -1,18 +0,0 @@ -#ifndef SNAKE_GLOBAL_H -#define SNAKE_GLOBAL_H - -#if defined(_MSC_VER) || defined(WIN64) || defined(_WIN64) || defined(__WIN64__) || defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__) -# define Q_DECL_EXPORT __declspec(dllexport) -# define Q_DECL_IMPORT __declspec(dllimport) -#else -# define Q_DECL_EXPORT __attribute__((visibility("default"))) -# define Q_DECL_IMPORT __attribute__((visibility("default"))) -#endif - -#if defined(SNAKE_LIBRARY) -# define SNAKE_EXPORT Q_DECL_EXPORT -#else -# define SNAKE_EXPORT Q_DECL_IMPORT -#endif - -#endif // SNAKE_GLOBAL_H diff --git a/src/Snake/snake_typedefs.h b/src/Snake/snake_typedefs.h new file mode 100644 index 0000000000000000000000000000000000000000..7788928384fbbc19c7e829f507f24099d6fa11a7 --- /dev/null +++ b/src/Snake/snake_typedefs.h @@ -0,0 +1,16 @@ +#pragma once + +#include +#include +#include + +namespace bg = boost::geometry; +namespace snake { + +typedef bg::model::point BoostPoint; +//typedef std::vector BoostPointList; +typedef bg::model::polygon BoostPolygon; +typedef bg::model::linestring BoostLineString; +typedef std::vector> Int64Matrix; + +} diff --git a/src/Wima/Geometry/WimaPolygonArray.h b/src/Wima/Geometry/WimaPolygonArray.h index f43f4ca15f98a67c5f85522bdcdfbf39b603b694..3815899e46b82127bc359e28f0bcf95213a5a75e 100644 --- a/src/Wima/Geometry/WimaPolygonArray.h +++ b/src/Wima/Geometry/WimaPolygonArray.h @@ -41,6 +41,12 @@ public: return _polygons; } + WimaPolygonArray &operator =(const WimaPolygonArray &other){ + this->_polygons = other._polygons; + this->_dirty = true; + return *this; + } + private: diff --git a/src/Wima/Snake/GeoPolygonArray.h b/src/Wima/Snake/GeoPolygonArray.h new file mode 100644 index 0000000000000000000000000000000000000000..d7a969c3c747c9d4099967037218f81326d07b1a --- /dev/null +++ b/src/Wima/Snake/GeoPolygonArray.h @@ -0,0 +1,5 @@ +#pragma once +#include "GeoTile.h" +#include "Wima/Geometry/WimaPolygonArray.h" + +using GeoPolygonArray = WimaPolygonArray; diff --git a/src/Wima/Snake/GeoTile.cpp b/src/Wima/Snake/GeoTile.cpp new file mode 100644 index 0000000000000000000000000000000000000000..e383644dcb1527edf2bd032b57e46c6170a71d2f --- /dev/null +++ b/src/Wima/Snake/GeoTile.cpp @@ -0,0 +1,23 @@ +#include "GeoTile.h" + +GeoTile::GeoTile() : WimaAreaData() +{ + +} + +GeoTile::GeoTile(const GeoTile &other) : WimaAreaData() +{ + *this = other; +} + +GeoTile &GeoTile::operator=(const GeoTile &other) +{ + this->assign(other); + return *this; + +} + +void GeoTile::assign(const GeoTile &other) +{ + WimaAreaData::assign(other); +} diff --git a/src/Wima/Snake/GeoTile.h b/src/Wima/Snake/GeoTile.h new file mode 100644 index 0000000000000000000000000000000000000000..1471934ca04cd48b7e0aeb9cc54274f5d9071efd --- /dev/null +++ b/src/Wima/Snake/GeoTile.h @@ -0,0 +1,19 @@ +#pragma once + +#include "Wima/Geometry/WimaAreaData.h" + +class GeoTile : public WimaAreaData +{ +public: + GeoTile(); + GeoTile(const GeoTile&other); + + QString type() const {return "Tile";} + GeoTile* Clone() const {return new GeoTile(*this);} + + GeoTile& operator=(const GeoTile &other); + +protected: + void assign(const GeoTile &other); +}; + diff --git a/src/Wima/Snake/SnakeTilesLocal.h b/src/Wima/Snake/PolygonArray.h similarity index 90% rename from src/Wima/Snake/SnakeTilesLocal.h rename to src/Wima/Snake/PolygonArray.h index 76f8b3702ac864ebc1669f61c41f36f4e624246a..48df733ee9d89c4e3338a72689f1a158da1aed0f 100644 --- a/src/Wima/Snake/SnakeTilesLocal.h +++ b/src/Wima/Snake/PolygonArray.h @@ -5,4 +5,4 @@ #include "Wima/Geometry/WimaPolygonArray.h" namespace MsgGroups = ROSBridge::MessageGroups; -typedef WimaPolygonArray SnakeTilesLocal; +typedef WimaPolygonArray PolygonArray; diff --git a/src/Wima/Snake/SnakeTiles.h b/src/Wima/Snake/SnakeTiles.h deleted file mode 100644 index b617acb325732902ddcd4a70cc33dbc0759d6189..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/SnakeTiles.h +++ /dev/null @@ -1,5 +0,0 @@ -#pragma once -#include "snaketile.h" -#include "Wima/Geometry/WimaPolygonArray.h" - -typedef WimaPolygonArray SnakeTiles; diff --git a/src/Wima/Snake/SnakeWorker.cc b/src/Wima/Snake/SnakeWorker.cc index 1cbe45cba0e59ef6f92094ae91f8482ad52359ae..4aa8dd3a680f6fddb1239160288c5f3634973a18 100644 --- a/src/Wima/Snake/SnakeWorker.cc +++ b/src/Wima/Snake/SnakeWorker.cc @@ -118,28 +118,28 @@ void SnakeWorker::run() const auto &cps = scenario.getTileCenterPoints(); for ( unsigned int i=0; i < tiles.size(); ++i ) { const auto &tile = tiles[i]; - SnakeTile Tile; + GeoTile geoTile; for ( const auto &vertex : tile) { QGeoCoordinate QVertex(vertex[0], vertex[1], vertex[2]); - Tile.append(QVertex); + geoTile.append(QVertex); } const auto ¢erPoint = cps[i]; - QGeoCoordinate QCenterPoint(centerPoint[0], centerPoint[1], centerPoint[2]); - Tile.setCenter(QCenterPoint); - _result.tiles.polygons().append(Tile); - _result.tileCenterPoints.append(QVariant::fromValue(QCenterPoint)); + 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 Tile; + Polygon2D localTile; for ( const auto &vertex : tile.outer()) { QPointF QVertex(vertex.get<0>(), vertex.get<1>()); - Tile.path().append(QVertex); + localTile.path().append(QVertex); } - _result.tilesLocal.polygons().append(Tile); + _result.tilesLocal.polygons().append(localTile); } } diff --git a/src/Wima/Snake/SnakeWorker.h b/src/Wima/Snake/SnakeWorker.h index f7382b758a5295e9b4511113a4e954c8affaa50f..4ea77f761e0433e8f7f705116f2d58a3f0c0311f 100644 --- a/src/Wima/Snake/SnakeWorker.h +++ b/src/Wima/Snake/SnakeWorker.h @@ -7,22 +7,22 @@ #include #include -#include "SnakeTiles.h" -#include "SnakeTilesLocal.h" +#include "GeoPolygonArray.h" +#include "PolygonArray.h" typedef QList QVariantList; typedef struct Result{ QVector waypoints; - SnakeTiles tiles; - SnakeTilesLocal tilesLocal; + GeoPolygonArray tiles; + PolygonArray tilesLocal; QVariantList tileCenterPoints; QGeoCoordinate origin; QVector arrivalPathIdx; QVector returnPathIdx; bool success; - QString errorMessage; + QString errorMessage; void clear(); }WorkerResult_t; diff --git a/src/Wima/Snake/snaketile.cpp b/src/Wima/Snake/snaketile.cpp deleted file mode 100644 index 4a8b6e6e35e6a75bb3362fe2ec5de3fe8ec14b3f..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/snaketile.cpp +++ /dev/null @@ -1,23 +0,0 @@ -#include "snaketile.h" - -SnakeTile::SnakeTile() : WimaAreaData() -{ - -} - -SnakeTile::SnakeTile(const SnakeTile &other) : WimaAreaData() -{ - *this = other; -} - -SnakeTile &SnakeTile::operator=(const SnakeTile &other) -{ - this->assign(other); - return *this; - -} - -void SnakeTile::assign(const SnakeTile &other) -{ - WimaAreaData::assign(other); -} diff --git a/src/Wima/Snake/snaketile.h b/src/Wima/Snake/snaketile.h deleted file mode 100644 index c41c649ddad8414b781071ec27a65c2c9b06dd32..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/snaketile.h +++ /dev/null @@ -1,19 +0,0 @@ -#pragma once - -#include "Wima/Geometry/WimaAreaData.h" - -class SnakeTile : public WimaAreaData -{ -public: - SnakeTile(); - SnakeTile(const SnakeTile&other); - - QString type() const {return "SnakeTile";} - SnakeTile* Clone() const {return new SnakeTile(*this);} - - SnakeTile& operator=(const SnakeTile &other); - -protected: - void assign(const SnakeTile &other); -}; - diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index 3d505df7d6f529159312f95c0f29db99ae10704d..e61f64c89acbd8015b22007818cb029ebdcfb054 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -112,7 +112,7 @@ WimaController::WimaController(QObject *parent) _eventTimer.start(EVENT_TIMER_INTERVAL); // Snake Worker Thread. - connect(&_snakeWorker, &SnakeWorker::finished, this, &WimaController::_snakeStoreWorkerResults); + connect(&_snakeWorker, &SnakeWorker::finished, this, &WimaController::_updateSnakeData); connect(this, &WimaController::nemoProgressChanged, this, &WimaController::_initStartSnakeWorker); connect(this, &QObject::destroyed, &this->_snakeWorker, &SnakeWorker::quit); @@ -818,13 +818,12 @@ void WimaController::_setSnakeCalcInProgress(bool inProgress) } } -void WimaController::_snakeStoreWorkerResults() -{ +void WimaController::_updateSnakeData() +{ + _setSnakeCalcInProgress(false); auto start = std::chrono::high_resolution_clock::now(); _snakeManager.clear(); - - const WorkerResult_t &r = _snakeWorker.result(); - _setSnakeCalcInProgress(false); + const auto& r = _snakeWorker.result(); if (!r.success) { //qgcApp()->showMessage(r.errorMessage); return; @@ -841,18 +840,43 @@ void WimaController::_snakeStoreWorkerResults() unsigned long endIdx = r.returnPathIdx.first(); for (unsigned long i = startIdx; i <= endIdx; ++i) { _snakeManager.push_back(r.waypoints[int(i)]); - } + } + auto end = std::chrono::high_resolution_clock::now(); + double duration = std::chrono::duration_cast(end-start).count(); + qWarning() << "WimaController: push_back waypoints execution time: " << duration << " ms."; + + start = std::chrono::high_resolution_clock::now(); _snakeManager.update(); + end = std::chrono::high_resolution_clock::now(); + duration = std::chrono::duration_cast(end-start).count(); + qWarning() << "WimaController: _snakeManager.update(); execution time: " << duration << " ms."; + + start = std::chrono::high_resolution_clock::now(); emit missionItemsChanged(); emit currentMissionItemsChanged(); emit currentWaypointPathChanged(); emit waypointPathChanged(); - - auto end = std::chrono::high_resolution_clock::now(); - double duration = std::chrono::duration_cast(end-start).count(); - qWarning() << "WimaController::_snakeStoreWorkerResults execution time: " << duration << " ms."; + end = std::chrono::high_resolution_clock::now(); + duration = std::chrono::duration_cast(end-start).count(); + qWarning() << "WimaController: gui update execution time: " << duration << " ms."; + + start = std::chrono::high_resolution_clock::now(); + _snakeOrigin = r.origin; + _snakeTileCenterPoints = r.tileCenterPoints; + _snakeTiles = r.tiles; + _snakeTilesLocal = r.tilesLocal; + end = std::chrono::high_resolution_clock::now(); + duration = std::chrono::duration_cast(end-start).count(); + qWarning() << "WimaController: tiles copy execution time: " << duration << " ms."; + + start = std::chrono::high_resolution_clock::now(); + emit snakeTilesChanged(); + emit snakeTileCenterPointsChanged(); + end = std::chrono::high_resolution_clock::now(); + duration = std::chrono::duration_cast(end-start).count(); + qWarning() << "WimaController: gui update 2 execution time: " << duration << " ms."; } void WimaController::_initStartSnakeWorker() diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index edf33e85c425d8687d5156f6a5fbebb5355eba99..1ae1ac0f500f032654c03923f87aed8136110dd3 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -29,8 +29,8 @@ #include "snake.h" #include "Snake/SnakeWorker.h" -#include "Snake/SnakeTiles.h" -#include "Snake/SnakeTilesLocal.h" +#include "Snake/GeoPolygonArray.h" +#include "Snake/PolygonArray.h" #include "Geometry/GeoPoint3D.h" #include "Snake/QNemoProgress.h" #include "Snake/QNemoHeartbeat.h" @@ -338,7 +338,7 @@ private slots: void _smartRTLCleanUp (bool flying); // Snake. void _setSnakeCalcInProgress (bool inProgress); - void _snakeStoreWorkerResults (); + void _updateSnakeData (); void _initStartSnakeWorker (); void _switchSnakeManager (QVariant variant); // Periodic tasks. @@ -402,9 +402,9 @@ private: // Snake bool _snakeCalcInProgress; SnakeWorker _snakeWorker; - GeoPoint _snakeOrigin; - SnakeTiles _snakeTiles; // tiles - SnakeTilesLocal _snakeTilesLocal; // tiles local coordinate system + GeoPoint _snakeOrigin; + GeoPolygonArray _snakeTiles; // tiles + PolygonArray _snakeTilesLocal; // tiles local coordinate system QVariantList _snakeTileCenterPoints; QNemoProgress _nemoProgress; // measurement progress QNemoHeartbeat _nemoHeartbeat; // measurement progress diff --git a/src/comm/ros_bridge/include/RosBridgeClient.h b/src/comm/ros_bridge/include/RosBridgeClient.h index ebcff85bffc7350ba11e749c8bf4938da3bc52cc..83e6febe0fe31401e6af2b1ccd3d91c197cd0a5c 100644 --- a/src/comm/ros_bridge/include/RosBridgeClient.h +++ b/src/comm/ros_bridge/include/RosBridgeClient.h @@ -5,10 +5,6 @@ * Author: Poom Pianpak */ - -#ifndef ROSBRIDGECPP_ROSBRIDGE_WS_CLIENT_HPP_ -#define ROSBRIDGECPP_ROSBRIDGE_WS_CLIENT_HPP_ - #include "Simple-WebSocket-Server/client_ws.hpp" #include "rapidjson/include/rapidjson/document.h"