Skip to content
GitLab
Commit 7c0976d9 authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files

not compilable

parent 5be50a5b
Hide whitespace changes
Inline Side-by-side
qgroundcontrol.pro
View file @ 7c0976d9
......@@ -408,7 +408,6 @@ FORMS += \
HEADERS += \
src/Snake/clipper/clipper.hpp \
src/Snake/flight_plan.h \
src/Snake/mapbox/feature.hpp \
src/Snake/mapbox/geometry.hpp \
src/Snake/mapbox/geometry/box.hpp \
......@@ -430,7 +429,6 @@ HEADERS += \
src/Snake/mapbox/variant.hpp \
src/Snake/mapbox/variant_io.hpp \
src/Snake/snake.h \
src/Snake/snake_geometry.h \
src/Snake/snake_typedefs.h \
src/Wima/Geometry/GeoPoint3D.h \
src/Wima/Geometry/Polygon2D.h \
......@@ -498,9 +496,7 @@ HEADERS += \
src/comm/utilities.h
SOURCES += \
src/Snake/clipper/clipper.cpp \
src/Snake/flight_plan.cpp \
src/Snake/snake.cpp \
src/Snake/snake_geometry.cpp \
src/Wima/Geometry/GeoPoint3D.cpp \
src/Wima/Geometry/PolygonArray.cc \
src/Wima/Snake/GeoTile.cpp \
......
src/Snake/flight_plan.cpp deleted 100644 → 0
View file @ 5be50a5b
#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;
}
src/Snake/flight_plan.h deleted 100644 → 0
View file @ 5be50a5b
#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
};
}
src/Snake/snake.cpp
View file @ 7c0976d9
......@@ -9,6 +9,14 @@
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include "clipper/clipper.hpp"
#define CLIPPER_SCALE 10000
#include "ortools/constraint_solver/routing.h"
#include "ortools/constraint_solver/routing_enums.pb.h"
#include "ortools/constraint_solver/routing_index_manager.h"
#include "ortools/constraint_solver/routing_parameters.h"
using namespace operations_research;
#ifndef NDEBUG
......@@ -187,68 +195,6 @@ void minimalBoundingBox(const BoostPolygon &polygon, BoundingBox &minBBox)
minBBox.corners = rotated_polygon;
}
void toBoost(const Point2D &point, BoostPoint &boost_point)
{
boost_point.set<0>(point[0]);
boost_point.set<1>(point[1]);
}
void fromBoost(const BoostPoint &boost_point, Point2D &point)
{
point[0] = boost_point.get<0>();
point[1] = boost_point.get<1>();
}
void toBoost(const Point2DList &point_list, BoostPolygon &boost_polygon)
{
for (auto vertex : point_list) {
BoostPoint boost_vertex;
toBoost(vertex, boost_vertex);
boost_polygon.outer().push_back(boost_vertex);
}
bg::correct(boost_polygon);
}
void fromBoost(const BoostPolygon &boost_polygon, Point2DList &point_list)
{
for (auto boost_vertex : boost_polygon.outer()) {
Point2D vertex;
fromBoost(boost_vertex, vertex);
point_list.push_back(vertex);
}
}
void rotateDeg(const Point2DList &point_list, Point2DList &rotated_point_list, double degree)
{
trans::rotate_transformer<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);
......@@ -435,37 +381,148 @@ void shortestPathFromGraph(const Matrix<double> &graph, size_t startIndex, size_
(void)ret;
}
}
//=========================================================================
// Tile calculation.
// Scenario 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) {
}Scenario::Scenario() :
_tileWidth(5)
, _tileHeight(5)
, _minTileArea(0)
, _needsUpdate(true)
{
}
void Scenario::setMeasurementArea(const BoostPolygon &area)
{
_needsUpdate = true;
_mArea = area;
}
void Scenario::setServiceArea(const BoostPolygon &area)
{
_needsUpdate = true;
_sArea = area;
}
void Scenario::setCorridor(const BoostPolygon &area)
{
_needsUpdate = true;
_corridor = area;
}
BoostPolygon &Scenario::measurementArea() {
_needsUpdate = true;
return _mArea;
}
BoostPolygon &Scenario::serviceArea() {
_needsUpdate = true;
return _sArea;
}
BoostPolygon &Scenario::corridor() {
_needsUpdate = true;
return _corridor;
}
const BoundingBox &Scenario::mAreaBoundingBox() const
{
return _mAreaBoundingBox;
}
const BoostPolygon &Scenario::measurementArea() const
{
return _mArea;
}
const BoostPolygon &Scenario::serviceArea() const
{
return _sArea;
}
const BoostPolygon &Scenario::corridor() const
{
return _corridor;
}
const BoostPolygon &Scenario::joinedArea() const {
return _jArea;
}
const vector<BoostPolygon> &Scenario::tiles() const{
return _tiles;
}
const BoostLineString &Scenario::tileCenterPoints() const{
return _tileCenterPoints;
}
const BoundingBox &Scenario::measurementAreaBBox() const{
return _mAreaBoundingBox;
}
const BoostPoint &Scenario::homePositon() const{
return _homePosition;
}
bool Scenario::update()
{
if ( !_needsUpdate )
return true;
if (!_calculateBoundingBox())
return false;
if (!_calculateTiles())
return false;
if (!_calculateJoinedArea())
return false;
_needsUpdate = false;
return true;
}
bool Scenario::_calculateBoundingBox()
{
minimalBoundingBox(_mArea, _mAreaBoundingBox);
return true;
}
/**
* Devides the (measurement area) bounding box into tiles and clips it to the measurement area.
*
* Devides the (measurement area) bounding box into tiles of width \p tileWidth and height \p tileHeight.
* Clips the resulting tiles to the measurement area. Tiles are rejected, if their area is smaller than \p minTileArea.
* The function assumes that \a _mArea and \a _mAreaBoundingBox have correct values. \see \ref Scenario::_areas2enu() and \ref
* Scenario::_calculateBoundingBox().
*
* @param tileWidth The width (>0) of a tile.
* @param tileHeight The heigth (>0) of a tile.
* @param minTileArea The minimal area (>0) of a tile.
*
* @return Returns true if successful.
*/
bool Scenario::_calculateTiles()
{
_tiles.clear();
_tileCenterPoints.clear();
if (_tileWidth <= 0 || _tileHeight <= 0 || _minTileArea < 0) {
errorString = "Parameters tileWidth, tileHeight, minTileArea must be positive.";
return false;
}
BoundingBox bbox;
minimalBoundingBox(mArea, bbox);
double bbox_width = bbox.width;
double bbox_height = bbox.height;
BoostPoint origin = bbox.corners.outer()[0];
double bbox_width = _mAreaBoundingBox.width;
double bbox_height = _mAreaBoundingBox.height;
BoostPoint origin = _mAreaBoundingBox.corners.outer()[0];
//cout << "Origin: " << origin[0] << " " << origin[1] << endl;
// Transform _measurementAreaENU polygon to bounding box coordinate system.
trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate(bbox.angle*180/M_PI);
// Transform _mArea polygon to bounding box coordinate system.
trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate(_mAreaBoundingBox.angle*180/M_PI);
trans::translate_transformer<double, 2, 2> translate(-origin.get<0>(), -origin.get<1>());
BoostPolygon translated_polygon;
BoostPolygon rotated_polygon;
boost::geometry::transform(_measurementAreaENU, translated_polygon, translate);
boost::geometry::transform(_mArea, translated_polygon, translate);
boost::geometry::transform(translated_polygon, rotated_polygon, rotate);
bg::correct(rotated_polygon);
......@@ -475,11 +532,12 @@ bool calculateTiles(const BoostPolygon &mArea,
size_t j_max = ceil(bbox_height/tileHeight);
if (i_max < 1 || j_max < 1) {
errorString = "tileWidth or tileHeight to large.";
errorString = "Tile width or Tile height to large.";
return false;
}
trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate_back(-bbox.angle*180/M_PI);
trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate_back(-_mAreaBoundingBox.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;
......@@ -510,13 +568,16 @@ bool calculateTiles(const BoostPolygon &mArea,
boost::geometry::transform(rotated_tile, translated_tile, translate_back);
// Store tile and calculate center point.
tiles.push_back(translated_tile);
_tiles.push_back(translated_tile);
BoostPoint tile_center;
polygonCenter(translated_tile, tile_center);
_tileCenterPoints.push_back(tile_center);
}
}
}
}
if (tiles.size() < 1){
if (_tiles.size() < 1){
errorString = "No tiles calculated. Is the minTileArea parameter large enough?";
return false;
}
......@@ -524,6 +585,100 @@ bool calculateTiles(const BoostPolygon &mArea,
return true;
}
bool Scenario::_calculateJoinedArea()
{
_jArea.clear();
// Measurement area and service area overlapping?
bool overlapingSerMeas = bg::intersects(_mArea, _sArea) ? true : false;
bool corridorValid = _corridor.outer().size() > 0 ? true : false;
// Check if corridor is connecting measurement area and service area.
bool corridor_is_connection = false;
if (corridorValid) {
// Corridor overlaping with measurement area?
if ( bg::intersects(_corridior, _mArea) ) {
// Corridor overlaping with service area?
if ( bg::intersects(_corridior, _sArea) ) {
corridor_is_connection = true;
}
}
}
// Are areas joinable?
std::deque<BoostPolygon> sol;
BoostPolygon partialArea = _mArea;
if (overlapingSerMeas){
if(corridor_is_connection){
bg::union_(partialArea, _corridior, sol);
}
} else if (corridor_is_connection){
bg::union_(partialArea, _corridior, sol);
} else {
errorString = "Areas are not overlapping";
return false;
}
if (sol.size() > 0) {
partialArea = sol[0];
sol.clear();
}
// Join areas.
bg::union_(partialArea, _sArea, sol);
if (sol.size() > 0) {
_jArea = sol[0];
} else {
return false;
}
return true;
}
double Scenario::minTileArea() const
{
return _minTileArea;
}
void Scenario::setMinTileArea(double minTileArea)
{
if ( minTileArea >= 0){
_needsUpdate = true;
_minTileArea = minTileArea;
}
}
double Scenario::tileHeight() const
{
return _tileHeight;
}
void Scenario::setTileHeight(double tileHeight)
{
if ( tileHeight > 0) {
_needsUpdate = true;
_tileHeight = tileHeight;
}
}
double Scenario::tileWidth() const
{
return _tileWidth;
}
void Scenario::setTileWidth(double tileWidth)
{
if ( tileWidth > 0 ){
_needsUpdate = true;
_tileWidth = tileWidth;
}
}
//=========================================================================
// Tile calculation.
//=========================================================================
bool joinAreas(const std::vector<BoostPolygon> &areas, BoostPolygon &joinedArea)
{
if (areas.size() < 1)
......@@ -531,15 +686,15 @@ bool joinAreas(const std::vector<BoostPolygon> &areas, BoostPolygon &joinedArea)
std::deque<std::size_t> idxList;
for(size_t i = 1; i < areas.size(); ++i)
idxList.push_back(i);
BoostPolygon partialArea = areas[0];
joinedArea = areas[0];
std::deque<BoostPolygon> sol;
while (idxList.size() > 0){
bool success = false;
for (auto it = idxList.begin(); it != idxList.end(); ++it){
bg::union_(partialArea, areas[*it], sol);
bg::union_(joinedArea, areas[*it], sol);
if (sol.size() > 0) {
partialArea = sol[0];
joinedArea = sol[0];
sol.clear;
idxList.erase(it);
success = true;
......@@ -549,23 +704,89 @@ bool joinAreas(const std::vector<BoostPolygon> &areas, BoostPolygon &joinedArea)
if ( !success )
return false;
}
return true;
}
BoundingBox::BoundingBox() :
width(0)
, height(0)
, angle(0)
{
}
void BoundingBox::clear()
{
width = 0;
height = 0;
angle = 0;
corners.clear();
}
Flightplan::Flightplan(ScenarioCPtr s, ProgressCPtr p)
: _scenario(s)
, _progress(p)
{
}
double Flightplan::lineDistance() const
{
return _lineDistance;
}
void Flightplan::setLineDistance(double lineDistance)
{
_lineDistance = lineDistance;
}
double Flightplan::minTransectLength() const
{
return _minTransectLength;
}
void Flightplan::setMinTransectLength(double minTransectLength)
{
_minTransectLength = minTransectLength;
}
Flightplan::ScenarioCPtr Flightplan::scenario() const
{
return _scenario;
}
void Flightplan::setScenario(ScenarioCPtr &scenario)
{
_scenario = scenario;
}
bool waypoints(const BoostPolygon &mArea,
const BoostPolygon &jArea,
std::vector<BoostPolygon> &tiles,
std::vector<long> &progress,
BoostPoint &home,
double lineDistance,
double minTransectLength,
std::vector<BoostPoint>,
size_t arrivalPathLength,
size_t returnPathLength)
Flightplan::ProgressCPtr Flightplan::progress() const
{
return _progress;
}
void Flightplan::setProgress(ProgressCPtr &progress)
{
_progress = progress;
}
struct Flightplan::RoutingDataModel{
Matrix<int64_t> distanceMatrix;
long numVehicles;
RoutingIndexManager::NodeIndex depot;
};
bool Flightplan::update()
{
_waypoints.clear();
_arrivalPath.clear();
_returnPath.clear();
#ifdef SHOW_TIME
auto start = std::chrono::high_resolution_clock::now();
#endif
if (!_generateTransects(mArea, lineDistance, minTransectLength, transects))
if (!_generateTransects())
return false;
#ifdef SHOW_TIME
auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - start);
......@@ -577,20 +798,21 @@ bool waypoints(const BoostPolygon &mArea,
// Route Transects using Google or-tools.
//=======================================
// Offset joined area.
const BoostPolygon &jArea = _scenario->joinedArea();
BoostPolygon jAreaOffset;
offsetPolygon(jArea, jAreaOffset, detail::offsetConstant);
// Create vertex list;
BoostLineString vertices;
size_t n_t = transects.size()*2;
size_t n_t = _transects.size()*2;
size_t n0 = n_t+1;
vertices.reserve(n0);
for (auto lstring : transects){
for (auto vertex : lstring){
for (auto& lstring : _transects){
for (auto& vertex : lstring){
vertices.push_back(vertex);
}
}
vertices.push_back(home);
vertices.push_back(_scenario->homePositon());
for (long i=0; i<long(jArea.outer().size())-1; ++i) {
vertices.push_back(jArea.outer()[i]);
......@@ -614,8 +836,7 @@ bool waypoints(const BoostPolygon &mArea,
#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);
......@@ -687,52 +908,78 @@ bool waypoints(const BoostPolygon &mArea,
index = solution->Value(routing.NextVar(index));
route.push_back(manager.IndexToNode(index).value());
}
long sz = route.size();
// Helper Lambda.
auto fromVertices = [&vertices](const std::vector<size_t> &idxArray,
std::vector<BoostPoint> &path){
for (size_t j=1; j<idxArray.size(); ++j)
path.push_back(vertices[idxArray[j]]);
};
// Fill arrival path.
_arrivalPath.push_back(vertices[route[0]]);
size_t idx0 = route[0];
size_t idx1 = route[1];
std::vector<size_t> pathIdx;
shortestPathFromGraph(connectionGraph, idx0, idx1, pathIdx);
fromVertices(pathIdx, _arrivalPath);
if (_arrivalPath.size() < 2)
return false;
// Connect transects
waypoint.push_back(vertices[route[0]]);
vector<size_t> pathIdx;
arrivalPathLength = 0;
for (long i=0; i<long(route.size())-1; ++i){
// Fill waypoints.
_waypoints.push_back(vertices[route[1]]);
for (long i=1; i<sz-2; ++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)
waypoints.push_back(vertices[pathIdx[j]]);
fromVertices(pathIdx, _waypoints);
}
_waypoints.push_back(vertices[route[sz-2]]);
returnPathLength = pathIdx.size();
return true;
if (_waypoints.size() < 2)
return false;
// Fill return path.
_returnPath.push_back(vertices[route[sz-2]]);
idx0 = route[sz-2];
idx1 = route[sz-1];
pathIdx.clear();
shortestPathFromGraph(connectionGraph, idx0, idx1, pathIdx);
fromVertices(pathIdx, _returnPath);
if (_returnPath.size() < 2)
return false;
return true;
}
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)
bool Flightplan::_generateTransects()
{
if (tiles.size() != progress.size()){
_transects.clear();
if (_scenario->tiles().size() != _progress->size()){
ostringstream strstream;
strstream << "Number of tiles ("
<< _scenario.getTilesENU().size()
<< _scenario->tiles().size()
<< ") is not equal to progress array length ("
<< _progress.size()
<< _progress->size()
<< ")";
errorString = strstream.str();
return false;
}
// Calculate processed tiles (_progress[i] == 100) and subtract them from measurement area.
size_t num_tiles = progress.size();
size_t num_tiles = _progress->size();
vector<BoostPolygon> processedTiles;
{
for (size_t i = 0; i < num_tiles; ++i) {
if (progress[i] == 100){
const auto &tiles = _scenario->tiles();
for (size_t i=0; i<num_tiles; ++i) {
if (_progress[i] == 100){
processedTiles.push_back(tiles[i]);
}
}
......@@ -743,13 +990,9 @@ bool FlightPlan::_generateTransects(double lineDistance,
// Convert measurement area and tiles to clipper path.
ClipperLib::Path mAreaClipper;
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)
}
);
for ( auto vertex : _scenario->measurementArea().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){
......@@ -761,8 +1004,7 @@ bool FlightPlan::_generateTransects(double lineDistance,
processedTilesClipper.push_back(path);
}
const BoundingBox bbox;
minimalBoundingBox(mArea, bbox);
const min_bbox_rt &bbox = _scenario->mAreaBoundingBox();
double alpha = bbox.angle;
double x0 = bbox.corners.outer()[0].get<0>();
double y0 = bbox.corners.outer()[0].get<1>();
......@@ -770,18 +1012,18 @@ bool FlightPlan::_generateTransects(double lineDistance,
double bboxHeight = bbox.height;
double delta = detail::offsetConstant;
size_t num_t = int(ceil((bboxHeight + 2*delta)/lineDistance)); // number of transects
size_t num_t = int(ceil((bboxHeight + 2*delta)/_lineDistance)); // number of transects
vector<double> yCoords;
yCoords.reserve(num_t);
double y = -delta;
for (size_t i=0; i < num_t; ++i) {
yCoords.push_back(y);
y += lineDistance;
y += _lineDistance;
}
// Generate transects and convert them to clipper path.
trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate_back(-alpha*180/M_PI);
trans::rotate_transformer<bg::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);
......@@ -833,15 +1075,61 @@ bool FlightPlan::_generateTransects(double lineDistance,
static_cast<double>(clipperTransect[1].Y)/CLIPPER_SCALE};
BoostLineString transect{v1, v2};
if (bg::length(transect) >= minTransectLength)
transects.push_back(transect);
if (bg::length(transect) >= _minTransectLength)
_transects.push_back(transect);
}
if (transects.size() < 1)
if (_transects.size() < 1)
return false;
return true;
}
void Flightplan::_generateRoutingModel(const BoostLineString &vertices,
const BoostPolygon &polygonOffset,
size_t n0,
Flightplan::RoutingDataModel &dataModel,
Matrix<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;
}
}
src/Snake/snake.h
View file @ 7c0976d9
......@@ -3,9 +3,9 @@
#include <vector>
#include <string>
#include <array>
#include <memory>
#include "snake_typedefs.h"
#include "snake_geometry.h"
#include <GeographicLib/Geocentric.hpp>
#include <GeographicLib/LocalCartesian.hpp>
......@@ -81,12 +81,16 @@ private:
std::vector<T> _matrix;
};
typedef struct {
struct BoundingBox{
BoundingBox();
void clear();
double width;
double height;
double angle;
BoostPolygon corners;
}BoundingBox;
};
template<class GeoPoint>
void toENU(const GeoPoint &origin, const GeoPoint &in, BoostPoint &out)
......@@ -144,8 +148,67 @@ void shortestPathFromGraph(const Matrix<double> &graph,
std::vector<size_t> &pathIdx);
//=========================================================================
// Tile calculation.
// Scenario.
//=========================================================================
class Scenario{
public:
Scenario();
void setMeasurementArea (const BoostPolygon &area);
void setServiceArea (const BoostPolygon &area);
void setCorridor (const BoostPolygon &area);
BoostPolygon &measurementArea();
BoostPolygon &serviceArea();
BoostPolygon &corridor();
const BoundingBox &mAreaBoundingBox() const;
const BoostPolygon &measurementArea() const;
const BoostPolygon &serviceArea()const;
const BoostPolygon &corridor()const;
double tileWidth() const;
void setTileWidth(double tileWidth);
double tileHeight() const;
void setTileHeight(double tileHeight);
double minTileArea() const;
void setMinTileArea(double minTileArea);
const BoostPolygon &joinedArea() const;
const vector<BoostPolygon> &tiles() const;
const BoostLineString &tileCenterPoints() const;
const BoundingBox &measurementAreaBBox() const;
const BoostPoint &homePositon() const;
bool update();
string errorString;
private:
bool _calculateBoundingBox();
bool _calculateTiles();
bool _calculateJoinedArea();
double _tileWidth;
double _tileHeight;
double _minTileArea;
mutable bool _needsUpdate;
BoostPolygon _mArea;
BoostPolygon _sArea;
BoostPolygon _corridor;
BoostPolygon _jArea;
BoundingBox _mAreaBoundingBox;
vector<BoostPolygon> _tiles;
BoostLineString _tileCenterPoints;
BoostPoint _homePosition;
};
template<class GeoPoint, template <class, class...> class Container>
void areaToEnu( const GeoPoint &origin,
const Container<GeoPoint> &in,
......@@ -156,7 +219,7 @@ void areaToEnu( const GeoPoint &origin,
toENU(origin, vertex, p);
out.outer().push_back(p);
}
bg::correct(_measurementAreaENU);
bg::correct(out);
}
template<class GeoPoint, template <class, class...> class Container>
......@@ -171,24 +234,61 @@ void areaFromEnu( const GeoPoint &origin,
}
}
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);
//========================================================================================
// Flightplan
//========================================================================================
//=========================================================================
// 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);
class Flightplan{
public:
using ScenarioPtr = shared_ptr<Scenario>;
using ProgressPtr = shared_ptr<vector<int>>;
Flightplan(ScenarioPtr s, ScenarioPtr p);
const vector<BoostPoint> &waypoints(void) const {return _waypoints;}
const vector<BoostPoint> &arrivalPath(void) const {return _arrivalPath;}
const vector<BoostPoint> &returnPath(void) const {return _returnPath;}
double lineDistance() const;
void setLineDistance(double lineDistance);
double minTransectLengthconst;
void setMinTransectLength(double minTransectLength);
ScenarioPtr scenario() const;
void setScenario(ScenarioPtr &scenario);
ProgressPtr progress() const;
void setProgress(ProgressPtr &progress);
bool update();
string errorString;
private:
// Search Filter to speed up routing.SolveWithParameters(...);
// Found here: http://www.lia.disi.unibo.it/Staff/MicheleLombardi/or-tools-doc/user_manual/manual/ls/ls_filtering.html
class SearchFilter;
struct RoutingDataModel;
bool _generateTransects();
void _generateRoutingModel(const BoostLineString &vertices,
const BoostPolygon &polygonOffset,
size_t n0,
RoutingDataModel &dataModel,
Matrix<double> &graph);
double _lineDistance;
double _minTransectLength;
shared_ptr<const Scenario> _pScenario;
shared_ptr<const vector<int>> _pProgress;
vector<BoostPoint> _waypoints;
vector<BoostPoint> _arrivalPath;
vector<BoostPoint> _returnPath;
vector<BoostLineString> _transects;
};
namespace detail {
const double offsetConstant = 0.1; // meter, polygon offset to compenstate for numerical inaccurracies.
......
src/Wima/Snake/SnakeWorker.cc
View file @ 7c0976d9
#include "SnakeWorker.h"
#include <QGeoCoordinate>
#include <QMutexLocker>
#include "snake_geometry.h"
#include "snake.h"
SnakeDataManager::SnakeDataManager(QObject *parent)
: QThread(parent)
, _pScenario(std::make_shared<snake::Scenario>())
, _pProgress(std::make_shared<std::vector<int>>())
, _pFlightplan(std::make_shared<snake::Flightplan>(_pScenario, _pProgress))
, _pData(std::make_shared<SnakeData>())
using namespace snake;
using namespace snake_geometry;
SnakeWorker::SnakeWorker(QObject *parent) : QThread(parent)
, _lineDistance(3)
, _minTransectLength(2)
, _tileWidth(5)
, _tileHeight(5)
, _minTileArea(3)
{
}
SnakeWorker::~SnakeWorker()
SnakeDataManager::~SnakeDataManager()
{
}
WorkerResult_t &SnakeWorker::result()
SnakeDataManager::SnakeDataPtr SnakeDataManager::snakeData()
{
return _result;
return _pData;
}
bool SnakeWorker::precondition() const
bool SnakeDataManager::precondition() const
{
return _mArea.size() > 0
&& _sArea.size() > 0
&& _corridor.size() > 0
&& _progress.size() > 0;
&& _pProgress->size() > 0;
}
double SnakeWorker::lineDistance() const
double SnakeDataManager::lineDistance() const
{
return _lineDistance;
_pFlightplan->lineDistance();
}
void SnakeWorker::run()
void SnakeDataManager::run()
{
// Reset _result struct.
_result.clear();
{
QMutexLocker lk(&_pData->m);
_pData->clear();
}
if ( !precondition())
if ( !precondition() )
return;
Scenario scenario;// Initialize scenario.
Area mArea;
for (auto vertex : _mArea){
mArea.geoPolygon.push_back(GeoPoint2D{vertex.latitude(), vertex.longitude()});
if ( _mArea.size() <= 0
|| _sArea.size() <= 0
|| _corridor.size() <= 0 ){
QMutexLocker lk(&_pData->m);
_pData->errorMessage = "At least one area invald. Size < 1.";
}
mArea.type = AreaType::MeasurementArea;
Area sArea;
for (auto vertex : _sArea){
sArea.geoPolygon.push_back(GeoPoint2D{vertex.latitude(), vertex.longitude()});
auto origin = _mArea.front();
// Measurement area update necessary.
if ( _mArea.size() != _pScenario->measurementArea().outer().size() ){
{
QMutexLocker lk(&_pData->m);
_pData->ENUorigin = origin;
}
for (auto geoVertex : _mArea){
snake::BoostPoint p;
snake::toENU(origin, geoVertex, p);
_pScenario->measurementArea().outer().push_back(p);
}
}
sArea.type = AreaType::ServiceArea;
Area corridor;
for (auto vertex : _corridor){
corridor.geoPolygon.push_back(GeoPoint2D{vertex.latitude(), vertex.longitude()});
// Service area update necessary.
if ( _sArea.size() != _pScenario->serviceArea().outer().size() ){
for (auto geoVertex : _sArea){
snake::BoostPoint p;
snake::toENU(origin, geoVertex, p);
_pScenario->serviceArea().outer().push_back(p);
}
}
corridor.type = AreaType::Corridor;
scenario.addArea(mArea);
scenario.addArea(sArea);
scenario.addArea(corridor);
// Service area update necessary.
if ( _corridor.size() != _pScenario->corridor().outer().size() ){
for (auto geoVertex : _corridor){
snake::BoostPoint p;
snake::toENU(origin, geoVertex, p);
_pScenario->corridor().outer().push_back(p);
}
}
if ( !scenario.update(_tileWidth, _tileHeight, _minTileArea) )
if ( !_pScenario->update() ){
{
QMutexLocker lk(&_pData->m);
for (auto c : _pScenario->errorString){
_pData->errorMessage.push_back(QChar(c));
}
}
return;
}
FlightPlan flightplan;
flightplan.setScenario(scenario);
flightplan.setProgress(_progress);
// Asynchronously update flightplan.
auto future = std::async(std::bind(&snake::Flightplan::update, _pFlightplan));
// Continue with storing scenario data in the mean time.
{
QMutexLocker lk(&_pData->m);
// Get tiles.
const auto &tiles = _pScenario->tiles();
const auto &centerPoints = _pScenario->tileCenterPoints();
for ( unsigned int i=0; i < tiles.size(); ++i ) {
const auto &tile = tiles[i];
GeoTile geoTile;
Polygon2D enuTile;
for ( size_t i = tile.outer().size(); i < tile.outer().size()-1; ++i) {
auto &p = tile.outer()[i];
QPointF enuVertex(p.get<0>(), p.get<1>(), 0.0);
QGeoCoordinate geoVertex;
snake::fromENU(origin, *it, geoVertex);
enuTile.polygon().push_back(enuVertex);
geoTile.push_back(QVertex);
}
const auto &boostPoint = centerPoints[i];
QPointF enuVertex(boostPoint.get<0>(), boostPoint.get<1>(), 0.0);
QGeoCoordinate geoVertex;
snake::fromENU(origin, enuVertex, geoVertex);
geoTile.setCenter(geoVertex);
_pData->tiles.polygons().push_back(geoTile);
_pData->tileCenterPoints.push_back(QVariant::fromValue(geoVertex));
_pData->tilesENU.polygons().push_back(enuTile);
_pData->tileCenterPointsENU.push_back(enuVertex);
}
}
future.wait();
// Trying to generate flight plan.
if ( !flightplan.generate(_lineDistance, _minTransectLength) ){
if ( !future.get() ){
// error
for (auto c : flightplan.errorString){
_result.errorMessage.push_back(QChar(c));
QMutexLocker lk(&_pData->m);
for (auto c : _pFlightplan->errorString){
_pData->errorMessage.push_back(QChar(c));
}
} else {
//success!!!
_result.success = true;
// Fill result struct.
auto& waypoints = flightplan.getWaypoints();
for (auto vertex : waypoints){
_result.waypoints.append(QGeoCoordinate(vertex[0], vertex[1], 0));
QMutexLocker lk(&_pData->m);
// Store waypoints.
for (auto boostVertex : _pFlightplan->waypoints()){
QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>(), 0.0};
QGeoCoordinate geoVertex;
snake::fromENU(origin, enuVertex, geoVertex);
_pData->waypointsENU.push_back(enuVertex);
_pData->waypoints.push_back(geoVertex);
}
auto& arrivalPathIdx = flightplan.getArrivalPathIdx();
for (auto idx : arrivalPathIdx){
_result.arrivalPathIdx.append(idx);
// Store arrival path.
for (auto boostVertex : _pFlightplan->arrivalPath()){
QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>(), 0.0};
QGeoCoordinate geoVertex;
snake::fromENU(origin, enuVertex, geoVertex);
_pData->arrivalPathENU.push_back(enuVertex);
_pData->arrivalPath.push_back(geoVertex);
}
auto& returnPathIdx = flightplan.getReturnPathIdx();
for (auto idx : returnPathIdx){
_result.returnPathIdx.append(idx);
// Store return path.
for (auto boostVertex : _pFlightplan->returnPath()){
QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>(), 0.0};
QGeoCoordinate geoVertex;
snake::fromENU(origin, enuVertex, geoVertex);
_pData->returnPathENU.push_back(enuVertex);
_pData->returnPath.push_back(geoVertex);
}
// Get tiles and origin.
auto origin = scenario.getOrigin();
_result.origin.setLatitude(origin[0]);
_result.origin.setLongitude(origin[1]);
_result.origin.setAltitude(origin[2]);
const auto &tiles = scenario.getTiles();
const auto &cps = scenario.getTileCenterPoints();
for ( unsigned int i=0; i < tiles.size(); ++i ) {
const auto &tile = tiles[i];
GeoTile geoTile;
for ( const auto &vertex : tile) {
QGeoCoordinate QVertex(vertex[0], vertex[1], vertex[2]);
geoTile.append(QVertex);
}
const auto &centerPoint = cps[i];
QGeoCoordinate c(centerPoint[0], centerPoint[1], centerPoint[2]);
geoTile.setCenter(c);
_result.tiles.polygons().append(geoTile);
_result.tileCenterPoints.append(QVariant::fromValue(c));
}
// Get local tiles.
const auto &tilesENU = scenario.getTilesENU();
for ( unsigned int i=0; i < tilesENU.size(); ++i ) {
const auto &tile = tilesENU[i];
Polygon2D localTile;
for ( const auto &vertex : tile.outer()) {
QPointF QVertex(vertex.get<0>(), vertex.get<1>());
localTile.path().append(QVertex);
}
_result.tilesLocal.polygons().append(localTile);
}
}
}
double SnakeWorker::minTransectLength() const
double SnakeDataManager::minTransectLength() const
{
return _minTransectLength;
return _pFlightplan->minTransectLength();
}
void SnakeWorker::setMinTransectLength(double minTransectLength)
void SnakeDataManager::setMinTransectLength(double minTransectLength)
{
if (minTransectLength > 0)
_minTransectLength = minTransectLength;
_pFlightplan->setMinTransectLength(minTransectLength);
}
double SnakeWorker::minTileArea() const
double SnakeDataManager::minTileArea() const
{
return _minTileArea;
return _pScenario->minTileArea();
}
void SnakeWorker::setLineDistance(double lineDistance)
void SnakeDataManager::setLineDistance(double lineDistance)
{
if (lineDistance > 0)
_lineDistance = lineDistance;
_pFlightplan->setLineDistance(lineDistance);
}
double SnakeWorker::tileWidth() const
double SnakeDataManager::tileWidth() const
{
return _tileWidth;
return _pScenario->tileWidth();
}
void SnakeWorker::setTileWidth(double tileWidth)
void SnakeDataManager::setTileWidth(double tileWidth)
{
if (tileWidth > 0)
_tileWidth = tileWidth;
_pScenario->setTileWidth(tileWidth);
}
double SnakeWorker::tileHeight() const
double SnakeDataManager::tileHeight() const
{
return _tileHeight;
return _pScenario->_tileHeight();
}
void SnakeWorker::setTileHeight(double tileHeight)
void SnakeDataManager::setTileHeight(double tileHeight)
{
if (tileHeight > 0)
_tileHeight = tileHeight;
_pScenario->setTileHeight(tileHeight);
}
void SnakeWorker::setMinTileArea(double minTileArea)
void SnakeDataManager::setMinTileArea(double minTileArea)
{
if (minTileArea > 0)
_minTileArea = minTileArea;
_pScenario->setMinTileArea(minTileArea);
}
void Result::clear()
SnakeData::SnakeData()
{
}
void SnakeData::clear()
{
this->success = false;
this->errorMessage = "";
this->waypoints.clear();
this->returnPathIdx.clear();
this->arrivalPathIdx.clear();
this->arrivalPath.clear();
this->returnPath.clear();
this->tiles.polygons().clear();
this->tilesLocal.polygons().clear();
this->tileCenterPoints.clear();
this->waypointsENU.clear();
this->arrivalPathENU.clear();
this->returnPathENU.clear();
this->tilesENU.polygons().clear();
this->tileCenterPoints.clear();
this->errorMessage = "";
}
void SnakeDataManager::setCorridor(const QList<QGeoCoordinate> &corridor)
{
_corridor.clear();
_pScenario->corridor().clear();
for (auto vertex : corridor) {
_corridor.push_back(vertex);
}
}
void SnakeDataManager::setProgress(const QList<IntType> &progress)
{
_pProgress->clear();
for (auto p : progress) {
assert(p >= -1 && p <= 100);
_pProgress->push_back(p);
}
}
void SnakeDataManager::setServiceArea(const QList<QGeoCoordinate> &serviceArea)
{
_sArea.clear();
_pScenario->serviceArea().clear();
for (auto vertex : serviceArea) {
_sArea.push_back(vertex);
}
}
void SnakeDataManager::setMeasurementArea(const QList<QGeoCoordinate> &measurementArea)
{
_mArea.clear();
_pScenario->measurementArea().clear();
for (auto vertex : measurementArea) {
_mArea.push_back(vertex);
}
}
src/Wima/Snake/SnakeWorker.h
View file @ 7c0976d9
......@@ -5,46 +5,63 @@
#include <QThread>
#include <QVector>
#include <QGeoCoordinate>
#include <QMutex>
#include <vector>
#include <memory>
#include "GeoPolygonArray.h"
#include "PolygonArray.h"
#include "SnakeWorker.h"
#include "QNemoProgress.h"
typedef QList<QVariant> QVariantList;
typedef struct Result{
namespace snake {
class Scenario;
class Flightplan;
}
struct SnakeData{
SnakeData();
QVector<QGeoCoordinate> waypoints;
QVector<QGeoCoordinate> arrivalPath;
QVector<QGeoCoordinate> returnPath;
QGeoCoordinate ENUorigin;
QVector<QGeoCoordinate> waypointsENU;
QVector<QGeoCoordinate> arrivalPathENU;
QVector<QGeoCoordinate> returnPathENU;
GeoPolygonArray tiles;
PolygonArray tilesLocal;
QVariantList tileCenterPoints;
QGeoCoordinate origin;
QVector<unsigned long> arrivalPathIdx;
QVector<unsigned long> returnPathIdx;
bool success;
PolygonArray tilesENU;
QVector<QGeoCoordinate> tileCenterPointsENU;
QString errorMessage;
mutable QMutex m;
void clear();
}WorkerResult_t;
};
class SnakeWorker : public QThread{
class SnakeDataManager : public QThread{
Q_OBJECT
public:
SnakeWorker(QObject *parent = nullptr);
~SnakeWorker() override;
using ProgressPtr = std::shared_ptr<std::vector<int>>;
using ScenarioPtr = std::shared_ptr<snake::Scenario>;
using FlightPlanPtr = std::shared_ptr<snake::Flightplan>;
using SnakeDataPtr = std::shared_ptr<SnakeData>;
SnakeDataManager(QObject *parent = nullptr);
~SnakeDataManager() override;
template<template<class, class...> class Container>
void setMeasurementArea (const Container<QGeoCoordinate> &measurementArea);
template<template<class, class...> class Container>
void setServiceArea (const Container<QGeoCoordinate> &serviceArea);
template<template<class, class...> class Container>
void setCorridor (const Container<QGeoCoordinate> &corridor);
template<template<class, class...> class Container, class IntType>
void setProgress (const Container<IntType> &progress);
WorkerResult_t &result();
void setMeasurementArea (const QList<QGeoCoordinate> &measurementArea);
void setServiceArea (const QList<QGeoCoordinate> &serviceArea);
void setCorridor (const QList<QGeoCoordinate> &corridor);
SnakeDataPtr snakeData();
double lineDistance() const;
void setLineDistance(double lineDistance);
......@@ -67,53 +84,20 @@ protected:
private:
bool precondition() const;
std::vector<QGeoCoordinate> _mArea;
std::vector<QGeoCoordinate> _sArea;
std::vector<QGeoCoordinate> _corridor;
std::vector<int> _progress;
double _lineDistance;
double _minTransectLength;
double _tileWidth;
double _tileHeight;
double _minTileArea;
WorkerResult_t _result;
ScenarioPtr _pScenario;
ProgressPtr _pProgress;
FlightPlanPtr _pFlightplan;
SnakeDataPtr _pData;
};
template<template<class, class...> class Container>
void SnakeWorker::setCorridor(const Container<QGeoCoordinate> &corridor)
{
_corridor.clear();
for (auto vertex : corridor) {
_corridor.push_back(vertex);
}
}
template<template<class, class...> class Container>
void SnakeWorker::setMeasurementArea(const Container<QGeoCoordinate> &measurementArea)
{
_mArea.clear();
for (auto vertex : measurementArea) {
_mArea.push_back(vertex);
}
}
template<template<class, class...> class Container>
void SnakeWorker::setServiceArea(const Container<QGeoCoordinate> &serviceArea)
{
_sArea.clear();
for (auto vertex : serviceArea) {
_sArea.push_back(vertex);
}
}
template<template<class, class...> class Container, class IntType>
void SnakeWorker::setProgress(const Container<IntType> &progress)
{
_progress.clear();
for (auto p : progress) {
assert(p >= -1 && p <= 100);
_progress.push_back(p);
}
}
src/Wima/WimaController.cc
View file @ 7c0976d9
......@@ -112,9 +112,9 @@ WimaController::WimaController(QObject *parent)
_eventTimer.start(EVENT_TIMER_INTERVAL);
// Snake Worker Thread.
connect(&_snakeWorker, &SnakeWorker::finished, this, &WimaController::_updateSnakeData);
connect(&_snakeWorker, &SnakeDataManager::finished, this, &WimaController::_updateSnakeData);
connect(this, &WimaController::nemoProgressChanged, this, &WimaController::_initStartSnakeWorker);
connect(this, &QObject::destroyed, &this->_snakeWorker, &SnakeWorker::quit);
connect(this, &QObject::destroyed, &this->_snakeWorker, &SnakeDataManager::quit);
// Snake.
connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_switchSnakeManager);
......
src/Wima/WimaController.h
View file @ 7c0976d9
......@@ -401,7 +401,7 @@ private:
// Snake
bool _snakeCalcInProgress;
SnakeWorker _snakeWorker;
SnakeDataManager _snakeWorker;
GeoPoint _snakeOrigin;
GeoPolygonArray _snakeTiles; // tiles
PolygonArray _snakeTilesLocal; // tiles local coordinate system
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment