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<int64_t> 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::milliseconds>(std::chrono::high_resolution_clock::now() - start);
+ cout << endl;
+ cout << "Execution time _generateTransects(): " << delta.count() << " ms" << endl;
+#endif
+
+ //=======================================
+ // Route Transects using Google or-tools.
+ //=======================================
+ // Offset joined area.
+ const BoostPolygon &jArea = _scenario.getJoineAreaENU();
+ BoostPolygon jAreaOffset;
+ offsetPolygon(jArea, jAreaOffset, detail::offsetConstant);
+
+ // Create vertex list;
+ BoostLineString vertices;
+ size_t n_t = _transects.size()*2;
+ size_t n0 = n_t+1;
+ vertices.reserve(n0);
+ for (auto lstring : _transects){
+ for (auto vertex : lstring){
+ vertices.push_back(vertex);
+ }
+ }
+ vertices.push_back(_scenario.getHomePositonENU());
+
+ for (long i=0; i<long(jArea.outer().size())-1; ++i) {
+ vertices.push_back(jArea.outer()[i]);
+ }
+ for (auto ring : jArea.inners()) {
+ for (auto vertex : ring)
+ vertices.push_back(vertex);
+ }
+ size_t n1 = vertices.size();
+ // Generate routing model.
+ RoutingDataModel dataModel;
+ Matrix<double> connectionGraph(n1, n1);
+
+#ifdef SHOW_TIME
+ start = std::chrono::high_resolution_clock::now();
+#endif
+ _generateRoutingModel(vertices, jAreaOffset, n0, dataModel, connectionGraph);
+#ifdef SHOW_TIME
+ delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start);
+ cout << "Execution time _generateRoutingModel(): " << delta.count() << " ms" << endl;
+#endif
+
+ // Create Routing Index Manager.
+ RoutingIndexManager manager(dataModel.distanceMatrix.getN(), dataModel.numVehicles,
+ dataModel.depot);
+ // Create Routing Model.
+ RoutingModel routing(manager);
+
+ // Create and register a transit callback.
+ const int transit_callback_index = routing.RegisterTransitCallback(
+ [&dataModel, &manager](int64 from_index, int64 to_index) -> int64 {
+ // Convert from routing variable Index to distance matrix NodeIndex.
+ auto from_node = manager.IndexToNode(from_index).value();
+ auto to_node = manager.IndexToNode(to_index).value();
+ return dataModel.distanceMatrix.get(from_node, to_node);
+ });
+
+ // Define cost of each arc.
+ routing.SetArcCostEvaluatorOfAllVehicles(transit_callback_index);
+
+
+ // Define Constraints.
+ size_t n = _transects.size()*2;
+ Solver *solver = routing.solver();
+ for (size_t i=0; i<n; i=i+2){
+// auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i));
+// auto idx1 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i+1));
+// auto cond0 = routing.NextVar(idx0)->IsEqual(idx1);
+// auto cond1 = routing.NextVar(idx1)->IsEqual(idx0);
+// auto c = solver->MakeNonEquality(cond0, cond1);
+// solver->AddConstraint(c);
+
+ // alternative
+ auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i));
+ auto idx1 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i+1));
+ auto cond0 = routing.NextVar(idx0)->IsEqual(idx1);
+ auto cond1 = routing.NextVar(idx1)->IsEqual(idx0);
+ vector<IntVar*> conds{cond0, cond1};
+ auto c = solver->MakeAllDifferent(conds);
+ solver->MakeRejectFilter();
+ solver->AddConstraint(c);
+ }
+
+
+ // Setting first solution heuristic.
+ RoutingSearchParameters searchParameters = DefaultRoutingSearchParameters();
+ searchParameters.set_first_solution_strategy(
+ FirstSolutionStrategy::PATH_CHEAPEST_ARC);
+ google::protobuf::Duration *tMax = new google::protobuf::Duration(); // seconds
+ tMax->set_seconds(10);
+ searchParameters.set_allocated_time_limit(tMax);
+
+ // Solve the problem.
+#ifdef SHOW_TIME
+ start = std::chrono::high_resolution_clock::now();
+#endif
+ const Assignment* solution = routing.SolveWithParameters(searchParameters);
+#ifdef SHOW_TIME
+ delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start);
+ cout << "Execution time routing.SolveWithParameters(): " << delta.count() << " ms" << endl;
+#endif
+
+ if (!solution || solution->Size() <= 1){
+ errorString = "Not able to solve the routing problem.";
+ return false;
+ }
+
+ // Extract waypoints from solution.
+ long index = routing.Start(0);
+ std::vector<size_t> route;
+ route.push_back(manager.IndexToNode(index).value());
+ while (!routing.IsEnd(index)){
+ index = solution->Value(routing.NextVar(index));
+ route.push_back(manager.IndexToNode(index).value());
+ }
+
+ // Connect transects
+#ifndef NDEBUG
+ _PathVertices = vertices;
+#endif
+
+ {
+ _waypointsENU.push_back(vertices[route[0]]);
+ vector<size_t> pathIdx;
+ long arrivalPathLength = 0;
+ for (long i=0; i<long(route.size())-1; ++i){
+ size_t idx0 = route[i];
+ size_t idx1 = route[i+1];
+ pathIdx.clear();
+ shortestPathFromGraph(connectionGraph, idx0, idx1, pathIdx);
+ if ( i==0 )
+ arrivalPathLength = pathIdx.size();
+ for (size_t j=1; j<pathIdx.size(); ++j)
+ _waypointsENU.push_back(vertices[pathIdx[j]]);
+ }
+
+
+ long returnPathLength = pathIdx.size();
+ for (long i=returnPathLength; i > 0; --i)
+ _returnPathIdx.push_back(_waypointsENU.size()-i);
+
+ for (long i=0; i < arrivalPathLength; ++i)
+ _arrivalPathIdx.push_back(i);
+ }
+
+ // Back transform waypoints.
+ GeoPoint3D origin{_scenario.getOrigin()};
+ for (auto vertex : _waypointsENU) {
+ GeoPoint3D geoVertex;
+ fromENU(origin, Point3D{vertex.get<0>(), vertex.get<1>(), 0}, geoVertex);
+ _waypoints.push_back(GeoPoint2D{geoVertex[0], geoVertex[1]});
+ }
+
+ return true;
+}
+
+bool FlightPlan::_generateTransects(double lineDistance, double minTransectLength)
+{
+ _transects.clear();
+ if (_scenario.getTilesENU().size() != _progress.size()){
+ ostringstream strstream;
+ strstream << "Number of tiles ("
+ << _scenario.getTilesENU().size()
+ << ") is not equal to progress array length ("
+ << _progress.size()
+ << ")";
+ errorString = strstream.str();
+ return false;
+ }
+
+ // Calculate processed tiles (_progress[i] == 100) and subtract them from measurement area.
+ size_t num_tiles = _progress.size();
+ vector<BoostPolygon> processedTiles;
+ {
+ const auto &tiles = _scenario.getTilesENU();
+ for (size_t i=0; i<num_tiles; ++i) {
+ if (_progress[i] == 100){
+ processedTiles.push_back(tiles[i]);
+ }
+ }
+
+ if (processedTiles.size() == num_tiles)
+ return true;
+ }
+
+ // Convert measurement area and tiles to clipper path.
+ ClipperLib::Path mAreaClipper;
+ for ( auto vertex : _scenario.getMeasurementAreaENU().outer() ){
+ mAreaClipper.push_back(ClipperLib::IntPoint{static_cast<ClipperLib::cInt>(vertex.get<0>()*CLIPPER_SCALE),
+ static_cast<ClipperLib::cInt>(vertex.get<1>()*CLIPPER_SCALE)});
+ }
+ vector<ClipperLib::Path> processedTilesClipper;
+ for (auto t : processedTiles){
+ ClipperLib::Path path;
+ for (auto vertex : t.outer()){
+ path.push_back(ClipperLib::IntPoint{static_cast<ClipperLib::cInt>(vertex.get<0>()*CLIPPER_SCALE),
+ static_cast<ClipperLib::cInt>(vertex.get<1>()*CLIPPER_SCALE)});
+ }
+ processedTilesClipper.push_back(path);
+ }
+
+ const min_bbox_rt &bbox = _scenario.getMeasurementAreaBBoxENU();
+ double alpha = bbox.angle;
+ double x0 = bbox.corners.outer()[0].get<0>();
+ double y0 = bbox.corners.outer()[0].get<1>();
+ double bboxWidth = bbox.width;
+ double bboxHeight = bbox.height;
+ double delta = detail::offsetConstant;
+
+ size_t num_t = int(ceil((bboxHeight + 2*delta)/lineDistance)); // number of transects
+ vector<double> yCoords;
+ yCoords.reserve(num_t);
+ double y = -delta;
+ for (size_t i=0; i < num_t; ++i) {
+ yCoords.push_back(y);
+ y += lineDistance;
+ }
+
+
+ // Generate transects and convert them to clipper path.
+ trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate_back(-alpha*180/M_PI);
+ trans::translate_transformer<double, 2, 2> translate_back(x0, y0);
+ vector<ClipperLib::Path> transectsClipper;
+ transectsClipper.reserve(num_t);
+ for (size_t i=0; i < num_t; ++i) {
+ // calculate transect
+ BoostPoint v1{-delta, yCoords[i]};
+ BoostPoint v2{bboxWidth+delta, yCoords[i]};
+ BoostLineString transect;
+ transect.push_back(v1);
+ transect.push_back(v2);
+
+ // transform back
+ BoostLineString temp_transect;
+ bg::transform(transect, temp_transect, rotate_back);
+ transect.clear();
+ bg::transform(temp_transect, transect, translate_back);
+
+
+ ClipperLib::IntPoint c1{static_cast<ClipperLib::cInt>(transect[0].get<0>()*CLIPPER_SCALE),
+ static_cast<ClipperLib::cInt>(transect[0].get<1>()*CLIPPER_SCALE)};
+ ClipperLib::IntPoint c2{static_cast<ClipperLib::cInt>(transect[1].get<0>()*CLIPPER_SCALE),
+ static_cast<ClipperLib::cInt>(transect[1].get<1>()*CLIPPER_SCALE)};
+ ClipperLib::Path path{c1, c2};
+ transectsClipper.push_back(path);
+ }
+
+ // Perform clipping.
+ // Clip transects to measurement area.
+ ClipperLib::Clipper clipper;
+ clipper.AddPath(mAreaClipper, ClipperLib::ptClip, true);
+ clipper.AddPaths(transectsClipper, ClipperLib::ptSubject, false);
+ ClipperLib::PolyTree clippedTransecsPolyTree1;
+ clipper.Execute(ClipperLib::ctIntersection, clippedTransecsPolyTree1, ClipperLib::pftNonZero, ClipperLib::pftNonZero);
+
+ // Subtract holes (tiles with measurement_progress == 100) from transects.
+ clipper.Clear();
+ for (auto child : clippedTransecsPolyTree1.Childs)
+ clipper.AddPath(child->Contour, ClipperLib::ptSubject, false);
+ clipper.AddPaths(processedTilesClipper, ClipperLib::ptClip, true);
+ ClipperLib::PolyTree clippedTransecsPolyTree2;
+ clipper.Execute(ClipperLib::ctDifference, clippedTransecsPolyTree2, ClipperLib::pftNonZero, ClipperLib::pftNonZero);
+
+ // Extract transects from PolyTree and convert them to BoostLineString
+ for (auto child : clippedTransecsPolyTree2.Childs){
+ ClipperLib::Path clipperTransect = child->Contour;
+ BoostPoint v1{static_cast<double>(clipperTransect[0].X)/CLIPPER_SCALE,
+ static_cast<double>(clipperTransect[0].Y)/CLIPPER_SCALE};
+ BoostPoint v2{static_cast<double>(clipperTransect[1].X)/CLIPPER_SCALE,
+ static_cast<double>(clipperTransect[1].Y)/CLIPPER_SCALE};
+
+ BoostLineString transect{v1, v2};
+ if (bg::length(transect) >= minTransectLength)
+ _transects.push_back(transect);
+
+ }
+
+ if (_transects.size() < 1)
+ return false;
+
+ return true;
+}
+
+void FlightPlan::_generateRoutingModel(const BoostLineString &vertices,
+ const BoostPolygon &polygonOffset,
+ size_t n0,
+ FlightPlan::RoutingDataModel &dataModel,
+ Matrix<double> &graph)
+{
+#ifdef SHOW_TIME
+ auto start = std::chrono::high_resolution_clock::now();
+#endif
+ graphFromPolygon(polygonOffset, vertices, graph);
+#ifdef SHOW_TIME
+ auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now()-start);
+ cout << "Execution time graphFromPolygon(): " << delta.count() << " ms" << endl;
+#endif
+// cout << endl;
+// for (size_t i=0; i<graph.size(); ++i){
+// vector<double> &row = graph[i];
+// for (size_t j=0; j<row.size();++j){
+// cout << "(" << i << "," << j << "):" << row[j] << " ";
+// }
+// cout << endl;
+// }
+// cout << endl;
+ Matrix<double> distanceMatrix(graph);
+#ifdef SHOW_TIME
+ start = std::chrono::high_resolution_clock::now();
+#endif
+ toDistanceMatrix(distanceMatrix);
+#ifdef SHOW_TIME
+ delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now()-start);
+ cout << "Execution time toDistanceMatrix(): " << delta.count() << " ms" << endl;
+#endif
+
+ dataModel.distanceMatrix.setDimension(n0, n0);
+ for (size_t i=0; i<n0; ++i){
+ dataModel.distanceMatrix.set(i, i, 0);
+ for (size_t j=i+1; j<n0; ++j){
+ dataModel.distanceMatrix.set(i, j, int64_t(distanceMatrix.get(i, j)*CLIPPER_SCALE));
+ dataModel.distanceMatrix.set(j, i, int64_t(distanceMatrix.get(i, j)*CLIPPER_SCALE));
+ }
+ }
+
+ dataModel.numVehicles = 1;
+ dataModel.depot = n0-1;
+}
diff --git a/src/Snake/flight_plan.h b/src/Snake/flight_plan.h
new file mode 100644
index 0000000000000000000000000000000000000000..fb79a7154b15aad963d8f363db5e42124ff6f3e2
--- /dev/null
+++ b/src/Snake/flight_plan.h
@@ -0,0 +1,59 @@
+#pragma once
+
+namespace flight_plan {
+
+
+
+//========================================================================================
+// FlightPlan
+//========================================================================================
+
+class FlightPlan{
+public:
+ FlightPlan();
+
+ void setScenario(Scenario &scenario) {_scenario = scenario;}
+ void setProgress(vector<int> &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<uint64_t> &getArrivalPathIdx(void) const {return _arrivalPathIdx;}
+ const vector<uint64_t> &getReturnPathIdx(void) const {return _returnPathIdx;}
+#ifndef NDEBUG
+ const BoostLineString &getPathVertices(void) const {return _PathVertices;}
+#endif
+
+ bool generate(double lineDistance, double minTransectLength);
+ const vector<BoostLineString> &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<double> &graph);
+
+ Scenario _scenario;
+ BoostLineString _waypointsENU;
+ GeoPoint2DList _waypoints;
+ vector<BoostLineString> _transects;
+ vector<int> _progress;
+ vector<uint64_t> _arrivalPathIdx;
+ vector<uint64_t> _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 <iostream>
#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 <mapbox/polylabel.hpp>
+#include <mapbox/geometry.hpp>
+
+#include <boost/geometry.hpp>
+#include <boost/geometry/geometries/polygon.hpp>
+#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
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<double> p;
+geometry::linear_ring<double> lr1;
+for (size_t i = 0; i < polygon.outer().size(); ++i) {
+ geometry::point<double> vertex(polygon.outer()[i].get<0>(), polygon.outer()[i].get<1>());
+ lr1.push_back(vertex);
+}
+p.push_back(lr1);
+geometry::point<double> 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<BoostPolygon2D>(convex_hull) << endl;
+
+ //# Compute edges (x2-x1,y2-y1)
+ std::vector<BoostPoint> 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<long> 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<double> 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<double>::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<bg::degree, double, 2, 2> rotate(angle*180/M_PI);
+ BoostPolygon hull_rotated;
+ bg::transform(convex_hull, hull_rotated, rotate);
+ //cout << "Convex hull rotated: " << bg::wkt<BoostPolygon2D>(hull_rotated) << endl;
+
+ bg::model::box<BoostPoint> box;
+ bg::envelope(hull_rotated, box);
+// cout << "Bounding box: " << bg::wkt<bg::model::box<BoostPoint2D>>(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<bg::degree, double, 2, 2> 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<bg::degree, double, 2, 2> 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<double> 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<BoostPolygon> 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<double> &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<double>::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<size_t> &elementPath,
+ std::function<double (const size_t, const size_t)> 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<double>::infinity();
+ };
+
+ // The list with all Nodes (elements)
+ std::vector<Node> 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<size_t> 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<double>::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<double> &graph)
+{
+ size_t n = graph.getN();
+
+ auto distance = [graph](size_t i, size_t j){
+ return graph.get(i,j);
+ };
+
+
+ std::vector<size_t> 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<double> &graph, size_t startIndex, size_t endIndex, std::vector<size_t> &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<BoostPolygon> &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<boost::geometry::degree, double, 2, 2> rotate(_mAreaBoundingBox.angle*180/M_PI);
+ trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate(bbox.angle*180/M_PI);
trans::translate_transformer<double, 2, 2> 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<boost::geometry::degree, double, 2, 2> rotate_back(-_mAreaBoundingBox.angle*180/M_PI);
+ trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate_back(-bbox.angle*180/M_PI);
trans::translate_transformer<double, 2, 2> 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<BoostPolygon> &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<std::size_t> idxList;
+ for(size_t i = 1; i < areas.size(); ++i)
+ idxList.push_back(i);
+ BoostPolygon partialArea = areas[0];
- // Are areas joinable?
std::deque<BoostPolygon> 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<int64_t> distanceMatrix;
- long numVehicles;
- RoutingIndexManager::NodeIndex depot;
-};
-
-FlightPlan::FlightPlan()
+bool waypoints(const BoostPolygon &mArea,
+ const BoostPolygon &jArea,
+ std::vector<BoostPolygon> &tiles,
+ std::vector<long> &progress,
+ BoostPoint &home,
+ double lineDistance,
+ double minTransectLength,
+ std::vector<BoostPoint>,
+ 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::milliseconds>(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<long(jArea.outer().size())-1; ++i) {
vertices.push_back(jArea.outer()[i]);
@@ -384,7 +614,8 @@ bool FlightPlan::generate(double lineDistance, double minTransectLength)
#endif
// Create Routing Index Manager.
- RoutingIndexManager manager(dataModel.distanceMatrix.getN(), dataModel.numVehicles,
+ RoutingIndexManager manager(dataModel.distanceMatrix.getN(),
+ dataModel.numVehicles,
dataModel.depot);
// Create Routing Model.
RoutingModel routing(manager);
@@ -458,14 +689,9 @@ bool FlightPlan::generate(double lineDistance, double minTransectLength)
}
// Connect transects
-#ifndef NDEBUG
- _PathVertices = vertices;
-#endif
-
- {
- _waypointsENU.push_back(vertices[route[0]]);
+ waypoint.push_back(vertices[route[0]]);
vector<size_t> pathIdx;
- long arrivalPathLength = 0;
+ arrivalPathLength = 0;
for (long i=0; i<long(route.size())-1; ++i){
size_t idx0 = route[i];
size_t idx1 = route[i+1];
@@ -474,33 +700,23 @@ bool FlightPlan::generate(double lineDistance, double minTransectLength)
if ( i==0 )
arrivalPathLength = pathIdx.size();
for (size_t j=1; j<pathIdx.size(); ++j)
- _waypointsENU.push_back(vertices[pathIdx[j]]);
- }
-
-
- long returnPathLength = pathIdx.size();
- for (long i=returnPathLength; i > 0; --i)
- _returnPathIdx.push_back(_waypointsENU.size()-i);
-
- for (long i=0; i < arrivalPathLength; ++i)
- _arrivalPathIdx.push_back(i);
- }
-
- // Back transform waypoints.
- GeoPoint3D origin{_scenario.getOrigin()};
- for (auto vertex : _waypointsENU) {
- GeoPoint3D geoVertex;
- fromENU(origin, Point3D{vertex.get<0>(), vertex.get<1>(), 0}, geoVertex);
- _waypoints.push_back(GeoPoint2D{geoVertex[0], geoVertex[1]});
+ 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<BoostPolygon> &tiles,
+ const std::vector<long> &progress,
+ vector<BoostLineString> &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<BoostPolygon> processedTiles;
{
- const auto &tiles = _scenario.getTilesENU();
- for (size_t i=0; i<num_tiles; ++i) {
- if (_progress[i] == 100){
+ for (size_t i = 0; i < num_tiles; ++i) {
+ if (progress[i] == 100){
processedTiles.push_back(tiles[i]);
}
}
@@ -528,9 +743,13 @@ bool FlightPlan::_generateTransects(double lineDistance, double minTransectLengt
// Convert measurement area and tiles to clipper path.
ClipperLib::Path mAreaClipper;
- for ( auto vertex : _scenario.getMeasurementAreaENU().outer() ){
- mAreaClipper.push_back(ClipperLib::IntPoint{static_cast<ClipperLib::cInt>(vertex.get<0>()*CLIPPER_SCALE),
- static_cast<ClipperLib::cInt>(vertex.get<1>()*CLIPPER_SCALE)});
+ for ( auto vertex : mArea.outer() ){
+ mAreaClipper.push_back(
+ ClipperLib::IntPoint{
+ static_cast<ClipperLib::cInt>(vertex.get<0>()*CLIPPER_SCALE),
+ static_cast<ClipperLib::cInt>(vertex.get<1>()*CLIPPER_SCALE)
+ }
+ );
}
vector<ClipperLib::Path> processedTilesClipper;
for (auto t : processedTiles){
@@ -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<double> &graph)
-{
-#ifdef SHOW_TIME
- auto start = std::chrono::high_resolution_clock::now();
-#endif
- graphFromPolygon(polygonOffset, vertices, graph);
-#ifdef SHOW_TIME
- auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now()-start);
- cout << "Execution time graphFromPolygon(): " << delta.count() << " ms" << endl;
-#endif
-// cout << endl;
-// for (size_t i=0; i<graph.size(); ++i){
-// vector<double> &row = graph[i];
-// for (size_t j=0; j<row.size();++j){
-// cout << "(" << i << "," << j << "):" << row[j] << " ";
-// }
-// cout << endl;
-// }
-// cout << endl;
- Matrix<double> distanceMatrix(graph);
-#ifdef SHOW_TIME
- start = std::chrono::high_resolution_clock::now();
-#endif
- toDistanceMatrix(distanceMatrix);
-#ifdef SHOW_TIME
- delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now()-start);
- cout << "Execution time toDistanceMatrix(): " << delta.count() << " ms" << endl;
-#endif
-
- dataModel.distanceMatrix.setDimension(n0, n0);
- for (size_t i=0; i<n0; ++i){
- dataModel.distanceMatrix.set(i, i, 0);
- for (size_t j=i+1; j<n0; ++j){
- dataModel.distanceMatrix.set(i, j, int64_t(distanceMatrix.get(i, j)*CLIPPER_SCALE));
- dataModel.distanceMatrix.set(j, i, int64_t(distanceMatrix.get(i, j)*CLIPPER_SCALE));
- }
- }
-
- dataModel.numVehicles = 1;
- dataModel.depot = n0-1;
-}
-
-Area::Area() : Area(GeoPoint2DList(), 0, 1, AreaType::MeasurementArea)
-{
-
-}
-
-Area::Area(const GeoPoint2DList &gP, AreaType tp) : Area(gP, 0, 1, tp)
-{
-
-}
-
-Area::Area(const GeoPoint2DList &gP, double alt, size_t l, AreaType tp) :
- geoPolygon(gP)
- , altitude(alt)
- , layers(l)
- , type(tp)
-{
-
-}
-
}
diff --git a/src/Snake/snake.h b/src/Snake/snake.h
index 923b1662b5ee89cb8c23e5d71155a035b0b9a13d..44dea00df34e13d9dbfd7ba19d91e9d4ac2d9c51 100644
--- a/src/Snake/snake.h
+++ b/src/Snake/snake.h
@@ -4,142 +4,191 @@
#include <string>
#include <array>
+#include "snake_typedefs.h"
#include "snake_geometry.h"
+#include <GeographicLib/Geocentric.hpp>
+#include <GeographicLib/LocalCartesian.hpp>
+
using namespace std;
-using namespace snake_geometry;
namespace snake {
- enum AreaType {MeasurementArea, ServiceArea, Corridor};
+//=========================================================================
+// Geometry stuff.
+//=========================================================================
+template<class T>
+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<T> _matrix;
+};
+
+typedef struct {
+ double width;
+ double height;
+ double angle;
+ BoostPolygon corners;
+}BoundingBox;
+
+template<class GeoPoint>
+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<class GeoPoint>
+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<double> &graph);
+void toDistanceMatrix(Matrix<double> &graph);
+bool dijkstraAlgorithm(const size_t numElements,
+ size_t startIndex,
+ size_t endIndex,
+ std::vector<size_t> &elementPath,
+ std::function<double (const size_t, const size_t)> distanceDij);
+void shortestPathFromGraph(const Matrix<double> &graph,
+ size_t startIndex,
+ size_t endIndex,
+ std::vector<size_t> &pathIdx);
+
+//=========================================================================
+// Tile calculation.
+//=========================================================================
+template<class GeoPoint, template <class, class...> class Container>
+void areaToEnu( const GeoPoint &origin,
+ const Container<GeoPoint> &in,
+ BoostPolygon &out )
+{
+ for(auto vertex : in) {
+ BoostPoint p;
+ toENU(origin, vertex, p);
+ out.outer().push_back(p);
+ }
+ bg::correct(_measurementAreaENU);
+}
+
+template<class GeoPoint, template <class, class...> class Container>
+void areaFromEnu( const GeoPoint &origin,
+ BoostPolygon &in,
+ const Container<GeoPoint> &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<BoostPolygon> &getTilesENU() {return _tilesENU;}
- const BoostLineString &getTileCenterPointsENU() {return _tileCenterPointsENU;}
- const min_bbox_rt &getMeasurementAreaBBoxENU() {return _mAreaBoundingBox;}
- const BoostPoint &getHomePositonENU() {return _homePositionENU;}
-
- const vector<GeoPoint3DList> &getTiles() {return _tiles;}
- const vector<GeoPoint3D> &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<BoostPolygon> _tilesENU;
- BoostLineString _tileCenterPointsENU;
-
- vector<GeoPoint3DList> _tiles;
- vector<GeoPoint3D> _tileCenterPoints;
-
- GeoPoint3D _geoOrigin;
- BoostPoint _homePositionENU;
-
- GeoPoint3D _homePosition;
- };
-
- //========================================================================================
- // FlightPlan
- //========================================================================================
-
- class FlightPlan{
- public:
- FlightPlan();
-
- void setScenario(Scenario &scenario) {_scenario = scenario;}
- void setProgress(vector<int> &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<uint64_t> &getArrivalPathIdx(void) const {return _arrivalPathIdx;}
- const vector<uint64_t> &getReturnPathIdx(void) const {return _returnPathIdx;}
-#ifndef NDEBUG
- const BoostLineString &getPathVertices(void) const {return _PathVertices;}
-#endif
-
- bool generate(double lineDistance, double minTransectLength);
- const vector<BoostLineString> &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<double> &graph);
-
- Scenario _scenario;
- BoostLineString _waypointsENU;
- GeoPoint2DList _waypoints;
- vector<BoostLineString> _transects;
- vector<int> _progress;
- vector<uint64_t> _arrivalPathIdx;
- vector<uint64_t> _returnPathIdx;
-
-#ifndef NDEBUG
- BoostLineString _PathVertices;
-#endif
- };
+bool calculateTiles(const BoostPolygon &mArea,
+ double tileWidth,
+ double tileHeight,
+ double minTileArea,
+ std::vector<BoostPolygon> &tiles,
+ std::string &errorString);
+bool joinAreas(const std::vector<BoostPolygon> &areas,
+ BoostPolygon &joinedArea);
+
+
+//=========================================================================
+// Waypoint calculation.
+//=========================================================================
+bool waypoints(const BoostPolygon &mArea, const BoostPolygon &jArea,
+ std::vector<BoostPolygon> &tiles,
+ BoostPoint &home,
+ double lineDistance,
+ double minTransectLength, std::vector<BoostPoint>, 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 <mapbox/polylabel.hpp>
-#include <mapbox/geometry.hpp>
-
-#include <boost/geometry.hpp>
-#include <boost/geometry/geometries/polygon.hpp>
-#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
-
-
-#include <GeographicLib/Geocentric.hpp>
-#include <GeographicLib/LocalCartesian.hpp>
-
-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<double> p;
- geometry::linear_ring<double> lr1;
- for (size_t i = 0; i < polygon.outer().size(); ++i) {
- geometry::point<double> vertex(polygon.outer()[i].get<0>(), polygon.outer()[i].get<1>());
- lr1.push_back(vertex);
- }
- p.push_back(lr1);
- geometry::point<double> 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<BoostPolygon2D>(convex_hull) << endl;
-
- //# Compute edges (x2-x1,y2-y1)
- std::vector<BoostPoint> 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<long> 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<double> 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<double>::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<bg::degree, double, 2, 2> rotate(angle*180/M_PI);
- BoostPolygon hull_rotated;
- bg::transform(convex_hull, hull_rotated, rotate);
- //cout << "Convex hull rotated: " << bg::wkt<BoostPolygon2D>(hull_rotated) << endl;
-
- bg::model::box<BoostPoint> box;
- bg::envelope(hull_rotated, box);
-// cout << "Bounding box: " << bg::wkt<bg::model::box<BoostPoint2D>>(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<bg::degree, double, 2, 2> 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<bg::degree, double, 2, 2> 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<double> 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<BoostPolygon> 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<double> &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<double>::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<size_t> &elementPath,
- std::function<double (const size_t, const size_t)> 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<double>::infinity();
- };
-
- // The list with all Nodes (elements)
- std::vector<Node> 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<size_t> 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<double>::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<double> &graph)
-{
- size_t n = graph.getN();
-
- auto distance = [graph](size_t i, size_t j){
- return graph.get(i,j);
- };
-
-
- std::vector<size_t> 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<double> &graph, size_t startIndex, size_t endIndex, std::vector<size_t> &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 <vector>
-#include <array>
-#include <boost/geometry.hpp>
-#include <bits/stl_set.h>
-
-namespace bg = boost::geometry;
-
-namespace snake_geometry {
-
-typedef std::array<double, 3> Point2D;
-typedef std::array<double, 3> Point3D;
-typedef std::array<double, 3> GeoPoint3D;
-typedef std::array<double, 2> GeoPoint2D;
-typedef std::vector<Point2D> Point2DList;
-typedef std::vector<Point3D> Point3DList;
-typedef std::vector<GeoPoint2D> GeoPoint2DList;
-typedef std::vector<GeoPoint3D> GeoPoint3DList;
-
-typedef bg::model::point<double, 2, bg::cs::cartesian> BoostPoint;
-//typedef std::vector<BoostPoint> BoostPointList;
-typedef bg::model::polygon<BoostPoint> BoostPolygon;
-typedef bg::model::linestring<BoostPoint> BoostLineString;
-typedef std::vector<std::vector<int64_t>> Int64Matrix;
-
-
-template<class T>
-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<T> _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<double> &graph);
-void toDistanceMatrix(Matrix<double> &graph);
-bool dijkstraAlgorithm(const size_t numElements,
- size_t startIndex,
- size_t endIndex,
- std::vector<size_t> &elementPath,
- std::function<double (const size_t, const size_t)> distanceDij);
-void shortestPathFromGraph(const Matrix<double> &graph,
- size_t startIndex,
- size_t endIndex,
- std::vector<size_t> &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 <vector>
+#include <array>
+#include <boost/geometry.hpp>
+
+namespace bg = boost::geometry;
+namespace snake {
+
+typedef bg::model::point<double, 2, bg::cs::cartesian> BoostPoint;
+//typedef std::vector<BoostPoint> BoostPointList;
+typedef bg::model::polygon<BoostPoint> BoostPolygon;
+typedef bg::model::linestring<BoostPoint> BoostLineString;
+typedef std::vector<std::vector<int64_t>> Int64Matrix;
+
+}
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<GeoTile, QVector>;
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<Polygon2D, QVector, MsgGroups::PolygonArrayGroup> SnakeTilesLocal;
+typedef WimaPolygonArray<Polygon2D, QVector, MsgGroups::PolygonArrayGroup> 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<SnakeTile, QVector> 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 <QGeoCoordinate>
#include <vector>
-#include "SnakeTiles.h"
-#include "SnakeTilesLocal.h"
+#include "GeoPolygonArray.h"
+#include "PolygonArray.h"
typedef QList<QVariant> QVariantList;
typedef struct Result{
QVector<QGeoCoordinate> waypoints;
- SnakeTiles tiles;
- SnakeTilesLocal tilesLocal;
+ GeoPolygonArray tiles;
+ PolygonArray tilesLocal;
QVariantList tileCenterPoints;
QGeoCoordinate origin;
QVector<unsigned long> arrivalPathIdx;
QVector<unsigned long> 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<std::chrono::milliseconds>(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<std::chrono::milliseconds>(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<std::chrono::milliseconds>(end-start).count();
- qWarning() << "WimaController::_snakeStoreWorkerResults execution time: " << duration << " ms.";
+ end = std::chrono::high_resolution_clock::now();
+ duration = std::chrono::duration_cast<std::chrono::milliseconds>(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<std::chrono::milliseconds>(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<std::chrono::milliseconds>(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"