Commit f3a5cb44 authored by Valentin Platzgummer's avatar Valentin Platzgummer

snake mod

parent fbf79828
......@@ -7,6 +7,7 @@
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include "clipper/clipper.hpp"
......@@ -26,13 +27,13 @@ using namespace operations_research;
namespace bg = boost::geometry;
namespace trans = bg::strategy::transform;
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(bg::cs::cartesian)
namespace snake {
//=========================================================================
// Geometry stuff.
//=========================================================================
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)
void polygonCenter(const BoostPolygon &polygon, BoostPoint &center)
{
using namespace mapbox;
......@@ -385,9 +386,9 @@ void shortestPathFromGraph(const Matrix<double> &graph, size_t startIndex, size_
// Scenario calculation.
//=========================================================================
}Scenario::Scenario() :
_tileWidth(5)
, _tileHeight(5)
, _minTileArea(0)
_tileWidth(5*bu::si::meter)
, _tileHeight(5*bu::si::meter)
, _minTileArea(0*bu::si::meter*bu::si::meter)
, _needsUpdate(true)
{
......@@ -446,21 +447,6 @@ const BoostPolygon &Scenario::corridor() const
return _corridor;
}
BoostPolygon &Scenario::measurementArea()
{
return _mArea;
}
BoostPolygon &Scenario::serviceArea()
{
return _sArea;
}
BoostPolygon &Scenario::corridor()
{
return _corridor;
}
const BoostPolygon &Scenario::joinedArea() const {
return _jArea;
}
......@@ -521,13 +507,17 @@ bool Scenario::_calculateTiles()
_tiles.clear();
_tileCenterPoints.clear();
if (_tileWidth <= 0 || _tileHeight <= 0 || _minTileArea < 0) {
if (_tileWidth <= 0*bu::si::meter
|| _tileHeight <= 0*bu::si::meter
|| _minTileArea <= 0*bu::si::meter*bu::si::meter
)
{
errorString = "Parameters tileWidth, tileHeight, minTileArea must be positive.";
return false;
}
double bbox_width = _mAreaBoundingBox.width;
double bbox_height = _mAreaBoundingBox.height;
double bboxWidth = _mAreaBoundingBox.width;
double bboxHeight = _mAreaBoundingBox.height;
BoostPoint origin = _mAreaBoundingBox.corners.outer()[0];
......@@ -543,23 +533,27 @@ bool Scenario::_calculateTiles()
//cout << bg::wkt<BoostPolygon2D>(rotated_polygon) << endl;
size_t i_max = ceil(bbox_width/tileWidth);
size_t j_max = ceil(bbox_height/tileHeight);
size_t iMax = ceil(bboxWidth/_tileWidth.value());
size_t jMax = ceil(bboxHeight/_tileHeight.value());
if (i_max < 1 || j_max < 1) {
errorString = "Tile width or Tile height to large.";
if (iMax < 1 || jMax < 1) {
std::stringstream ss;
ss << "Tile width or Tile height to large. "
<< "tile width = " << _tileWidth << ", "
<< "tile height = " << _tileHeight << "." << std::endl;
errorString = ss.str();
return false;
}
trans::rotate_transformer<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;
double x_max = x_min + tileWidth;
for (size_t j = 0; j < j_max; ++j){
double y_min = tileHeight*j;
double y_max = y_min + tileHeight;
for (size_t i = 0; i < iMax; ++i){
double x_min = _tileWidth.value()*i;
double x_max = x_min + _tileWidth.value();
for (size_t j = 0; j < jMax; ++j){
double y_min = _tileHeight.value()*j;
double y_max = y_min + _tileHeight.value();
BoostPolygon tile_unclipped;
tile_unclipped.outer().push_back(BoostPoint{x_min, y_min});
......@@ -575,7 +569,7 @@ bool Scenario::_calculateTiles()
for (BoostPolygon t : boost_tiles)
{
if (bg::area(t) > minTileArea){
if (bg::area(t) > _minTileArea.value()){
// Transform boost_tile to world coordinate system.
BoostPolygon rotated_tile;
BoostPolygon translated_tile;
......@@ -612,9 +606,9 @@ bool Scenario::_calculateJoinedArea()
bool corridor_is_connection = false;
if (corridorValid) {
// Corridor overlaping with measurement area?
if ( bg::intersects(_corridior, _mArea) ) {
if ( bg::intersects(_corridor, _mArea) ) {
// Corridor overlaping with service area?
if ( bg::intersects(_corridior, _sArea) ) {
if ( bg::intersects(_corridor, _sArea) ) {
corridor_is_connection = true;
}
}
......@@ -625,10 +619,10 @@ bool Scenario::_calculateJoinedArea()
BoostPolygon partialArea = _mArea;
if (overlapingSerMeas){
if(corridor_is_connection){
bg::union_(partialArea, _corridior, sol);
bg::union_(partialArea, _corridor, sol);
}
} else if (corridor_is_connection){
bg::union_(partialArea, _corridior, sol);
bg::union_(partialArea, _corridor, sol);
} else {
errorString = "Areas are not overlapping";
return false;
......@@ -651,40 +645,40 @@ bool Scenario::_calculateJoinedArea()
return true;
}
double Scenario::minTileArea() const
Area Scenario::minTileArea() const
{
return _minTileArea;
}
void Scenario::setMinTileArea(double minTileArea)
void Scenario::setMinTileArea(Area minTileArea)
{
if ( minTileArea >= 0){
if ( minTileArea >= 0*bu::si::meter*bu::si::meter){
_needsUpdate = true;
_minTileArea = minTileArea;
}
}
double Scenario::tileHeight() const
Length Scenario::tileHeight() const
{
return _tileHeight;
}
void Scenario::setTileHeight(double tileHeight)
void Scenario::setTileHeight(Length tileHeight)
{
if ( tileHeight > 0) {
if ( tileHeight > 0*bu::si::meter) {
_needsUpdate = true;
_tileHeight = tileHeight;
}
}
double Scenario::tileWidth() const
Length Scenario::tileWidth() const
{
return _tileWidth;
}
void Scenario::setTileWidth(double tileWidth)
void Scenario::setTileWidth(Length tileWidth)
{
if ( tileWidth > 0 ){
if ( tileWidth > 0*bu::si::meter ){
_needsUpdate = true;
_tileWidth = tileWidth;
}
......@@ -710,7 +704,7 @@ bool joinAreas(const std::vector<BoostPolygon> &areas, BoostPolygon &joinedArea)
bg::union_(joinedArea, areas[*it], sol);
if (sol.size() > 0) {
joinedArea = sol[0];
sol.clear;
sol.clear();
idxList.erase(it);
success = true;
break;
......@@ -739,112 +733,237 @@ void BoundingBox::clear()
corners.clear();
}
Flightplan::Flightplan(ScenarioCPtr s, ProgressCPtr p)
: _scenario(s)
, _progress(p)
bool flight_plan::transectsFromScenario(Length distance,
Length minLength,
Angle angle,
const Scenario &scenario,
const Progress &p,
flight_plan::Transects &t,
string &errorString)
{
// Rotate measurement area by angle and calculate bounding box.
auto &mArea = scenario.measurementArea();
BoostPolygon mAreaRotated;
trans::rotate_transformer<bg::degree, double, 2, 2> rotate(-angle.value()*180/M_PI);
bg::transform(mArea, mAreaRotated, rotate);
BoostBox box;
boost::geometry::envelope(mAreaRotated, box);
double x0 = box.min_corner().get<0>();
double y0 = box.min_corner().get<1>();
double x1 = box.max_corner().get<0>();
double y1 = box.max_corner().get<1>();
// Generate transects and convert them to clipper path.
size_t num_t = int(ceil((y1 - y0)/distance.value())); // number of transects
trans::rotate_transformer<bg::degree, double, 2, 2> rotate_back(angle.value()*180/M_PI);
vector<ClipperLib::Path> transectsClipper;
transectsClipper.reserve(num_t);
for (size_t i=0; i < num_t; ++i) {
// calculate transect
BoostPoint v1{x0, y0 + i*distance.value()};
BoostPoint v2{x1, y0 + i*distance.value()};
BoostLineString transect;
transect.push_back(v1);
transect.push_back(v2);
}
// transform back
BoostLineString temp_transect;
bg::transform(transect, temp_transect, rotate_back);
double Flightplan::lineDistance() const
{
return _lineDistance;
}
void Flightplan::setLineDistance(double lineDistance)
{
_lineDistance = lineDistance;
}
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);
}
double Flightplan::minTransectLength() const
{
return _minTransectLength;
}
if (transectsClipper.size() == 0){
std::stringstream ss;
ss << "Not able to generate transects. Parameter: distance = " << distance << std::endl;
errorString = ss.str();
return false;
}
void Flightplan::setMinTransectLength(double minTransectLength)
{
_minTransectLength = minTransectLength;
}
// Convert measurement area 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)});
}
Flightplan::ScenarioCPtr Flightplan::scenario() const
{
return _scenario;
}
// Perform clipping.
// Clip transects to measurement area.
ClipperLib::Clipper clipper;
clipper.AddPath(mAreaClipper, ClipperLib::ptClip, true);
clipper.AddPaths(transectsClipper, ClipperLib::ptSubject, false);
ClipperLib::PolyTree clippedTransecs;
clipper.Execute(ClipperLib::ctIntersection, clippedTransecs, ClipperLib::pftNonZero, ClipperLib::pftNonZero);
auto &transects = clippedTransecs;
bool ignoreProgress = p.size() != scenario.tiles().size();
ClipperLib::PolyTree clippedTransecs2;
if ( !ignoreProgress ) {
// Calculate processed tiles (_progress[i] == 100) and subtract them from measurement area.
size_t numTiles = p.size();
vector<BoostPolygon> processedTiles;
const auto &tiles = scenario.tiles();
for (size_t i=0; i<numTiles; ++i) {
if (p[i] == 100){
processedTiles.push_back(tiles[i]);
}
}
void Flightplan::setScenario(ScenarioCPtr &scenario)
{
_scenario = scenario;
}
if (processedTiles.size() != numTiles){
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);
}
Flightplan::ProgressCPtr Flightplan::progress() const
{
return _progress;
}
// Subtract holes (tiles with measurement_progress == 100) from transects.
clipper.Clear();
for (auto &child : clippedTransecs.Childs)
clipper.AddPath(child->Contour, ClipperLib::ptSubject, false);
clipper.AddPaths(processedTilesClipper, ClipperLib::ptClip, true);
clipper.Execute(ClipperLib::ctDifference, clippedTransecs2, ClipperLib::pftNonZero, ClipperLib::pftNonZero);
transects = clippedTransecs2;
}
}
void Flightplan::setProgress(ProgressCPtr &progress)
{
_progress = progress;
// Extract transects from PolyTree and convert them to BoostLineString
for (auto &child : transects.Childs){
auto &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) >= minLength.value())
t.push_back(transect);
}
if (t.size() == 0){
std::stringstream ss;
ss << "Not able to generate transects. Parameter: minLength = " << minLength << std::endl;
errorString = ss.str();
return false;
}
return true;
}
struct Flightplan::RoutingDataModel{
struct RoutingDataModel{
Matrix<int64_t> distanceMatrix;
long numVehicles;
RoutingIndexManager::NodeIndex depot;
};
bool Flightplan::update()
{
_waypoints.clear();
_arrivalPath.clear();
_returnPath.clear();
void generateRoutingModel(const BoostLineString &vertices,
const BoostPolygon &polygonOffset,
size_t n0,
RoutingDataModel &dataModel,
Matrix<double> &graph){
#ifdef SHOW_TIME
auto start = std::chrono::high_resolution_clock::now();
#endif
if (!_generateTransects())
return false;
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
Matrix<double> distanceMatrix(graph);
#ifdef SHOW_TIME
start = std::chrono::high_resolution_clock::now();
#endif
toDistanceMatrix(distanceMatrix);
#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;
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 = 0;
}
bool flight_plan::route(const BoostPolygon &area,
const flight_plan::Transects &transects,
Transects &transectsRouted,
flight_plan::Route &route,
string &errorString)
{
//=======================================
// 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 n0 = n_t+1;
size_t n0 = 0;
for ( const auto &t : transects){
if (t.size() >= 2){
n0 += 2;
} else {
n0 += 1;
}
}
vertices.reserve(n0);
for (auto& lstring : _transects){
for (auto& vertex : lstring){
vertices.push_back(vertex);
struct TransectInfo{
TransectInfo(size_t n, bool f)
: index(n)
, front(f)
{}
size_t index;
bool front;
};
std::vector<TransectInfo> transectInfoList;
size_t idx = 0;
for ( size_t i=0; i<transects.size(); ++i){
const auto &t = transects[i];
vertices.push_back(t.front());
transectInfoList.push_back(TransectInfo{i, true});
if (t.size() >= 2){
vertices.push_back(t.back());
transectInfoList.push_back(TransectInfo{i, false});
}
}
vertices.push_back(_scenario->homePositon());
for (long i=0; i<long(jArea.outer().size())-1; ++i) {
vertices.push_back(jArea.outer()[i]);
for (long i=0; i<long(area.outer().size())-1; ++i) {
vertices.push_back(area.outer()[i]);
}
for (auto ring : jArea.inners()) {
for (auto vertex : ring)
for (auto &ring : area.inners()) {
for (auto &vertex : ring)
vertices.push_back(vertex);
}
size_t n1 = vertices.size();
// Generate routing model.
RoutingDataModel dataModel;
Matrix<double> connectionGraph(n1, n1);
Matrix<double> connectionGraph(n1, n1);
// Offset joined area.
BoostPolygon areaOffset;
offsetPolygon(area, areaOffset, detail::offsetConstant);
#ifdef SHOW_TIME
start = std::chrono::high_resolution_clock::now();
#endif
_generateRoutingModel(vertices, jAreaOffset, n0, dataModel, connectionGraph);
generateRoutingModel(vertices, areaOffset, 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;
......@@ -857,7 +976,7 @@ bool Flightplan::update()
RoutingModel routing(manager);
// Create and register a transit callback.
const int transit_callback_index = routing.RegisterTransitCallback(
const int transitCallbackIndex = routing.RegisterTransitCallback(
[&dataModel, &manager](int64 from_index, int64 to_index) -> int64 {
// Convert from routing variable Index to distance matrix NodeIndex.
auto from_node = manager.IndexToNode(from_index).value();
......@@ -866,32 +985,30 @@ bool Flightplan::update()
});
// Define cost of each arc.
routing.SetArcCostEvaluatorOfAllVehicles(transit_callback_index);
routing.SetArcCostEvaluatorOfAllVehicles(transitCallbackIndex);
// Define Constraints.
size_t n = _transects.size()*2;
// Define Constraints (this constraints have a huge impact on the
// solving time, improvments could be done, e.g. SearchFilter).
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);
size_t index = 0;
for (size_t i=0; i<transects.size(); ++i){
const auto& t = transects[i];
if (t.size() >= 2){
auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(index));
auto idx1 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(index+1));
auto cond0 = routing.NextVar(idx0)->IsEqual(idx1);
auto cond1 = routing.NextVar(idx1)->IsEqual(idx0);
vector<IntVar*> conds{cond0, cond1};
auto c = solver->MakeAllDifferent(conds);
solver->MakeRejectFilter();
solver->AddConstraint(c);
index += 2;
} else {
index += 1;
}
}
// Setting first solution heuristic.
RoutingSearchParameters searchParameters = DefaultRoutingSearchParameters();
searchParameters.set_first_solution_strategy(
......@@ -915,236 +1032,42 @@ bool Flightplan::update()
return false;
}
// Extract waypoints from solution.
long index = routing.Start(0);
std::vector<size_t> route;
route.push_back(manager.IndexToNode(index).value());
// Extract index list from solution.
index = routing.Start(0);
std::vector<size_t> route_idx;
route_idx.push_back(manager.IndexToNode(index).value());
while (!routing.IsEnd(index)){
index = solution->Value(routing.NextVar(index));
route.push_back(manager.IndexToNode(index).value());
route_idx.push_back(manager.IndexToNode(index).value());
}
long sz = route.size();
// Helper Lambda.
auto fromVertices = [&vertices](const std::vector<size_t> &idxArray,
auto idx2Vertex = [&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]]);
for (auto idx : idxArray)
path.push_back(vertices[idx]);
};
// 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;
// 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);
fromVertices(pathIdx, _waypoints);
}
_waypoints.push_back(vertices[route[sz-2]]);
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()
{
_transects.clear();
if (_scenario->tiles().size() != _progress->size()){
ostringstream strstream;
strstream << "Number of tiles ("
<< _scenario->tiles().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->tiles();
for (size_t i=0; i<num_tiles; ++i) {
if (_progress[i] == 100){
processedTiles.push_back(tiles[i]);
// Construct route.
for (size_t i=0; i<route_idx.size()-1; ++i){
size_t idx0 = route_idx[i];
size_t idx1 = route_idx[i+1];
const auto &info1 = transectInfoList[idx0];
const auto &info2 = transectInfoList[idx0];
if ( info.front ){
const auto &t = transects[info.index];
for ( auto it = t.begin(); it < t.end()-1; ++it){
route.push_back(*it);
}
transectsRouted.push_back(t);
} else {
std::vector<size_t> idxList;
shortestPathFromGraph(connectionGraph, idx0, idx1, idxList);
idxList.pop_back();
idx2Vertex(idxList, route);
}
if (processedTiles.size() == num_tiles)
return true;
}
// Convert measurement area and tiles to clipper path.
ClipperLib::Path mAreaClipper;
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){
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->mAreaBoundingBox();
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<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);
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;
}
}
} // namespace snake
......@@ -158,10 +158,6 @@ class Scenario{
void setServiceArea (const BoostPolygon &area);
void setCorridor (const BoostPolygon &area);
BoostPolygon &measurementArea();
BoostPolygon &serviceArea();
BoostPolygon &corridor();
const BoundingBox &mAreaBoundingBox() const;
const BoostPolygon &measurementArea() const;
......@@ -242,60 +238,31 @@ bool joinAreas(const std::vector<BoostPolygon> &areas,
BoostPolygon &joinedArea);
//========================================================================================
// Flightplan
// flight_plan
//========================================================================================
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.
}
namespace flight_plan{
using Transects = vector<BoostLineString>;
using Progress = vector<int>;
using Route = vector<BoostPoint>;
bool transectsFromScenario(Length distance,
Length minLength,
Angle angle,
const Scenario &scenario,
const Progress &p,
Transects &t,
string &errorString);
bool route(const BoostPolygon &area,
const Transects &transects,
Transects &transectsRouted,
Route &route,
string &errorString);
} // namespace flight_plan
namespace detail {
const double offsetConstant = 0.1; // meter, polygon offset to compenstate for numerical inaccurracies.
} // namespace detail
}
......@@ -3,7 +3,13 @@
#include <vector>
#include <array>
#include <boost/geometry.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/si/io.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
#include <boost/units/systems/si/prefixes.hpp>
#include <boost/units/systems/angle/degrees.hpp>
namespace bg = boost::geometry;
namespace bu = boost::units;
......@@ -14,8 +20,10 @@ typedef bg::model::point<double, 2, bg::cs::cartesian> BoostPoint;
typedef bg::model::polygon<BoostPoint> BoostPolygon;
typedef bg::model::linestring<BoostPoint> BoostLineString;
typedef std::vector<std::vector<int64_t>> Int64Matrix;
typedef bg::model::box<BoostPoint> BoostBox;
typedef bu::quantity<bu::si::length> Length;
typedef bu::quantity<bu::si::area> Area;
typedef bu::quantity<bu::si::length, double> Length;
typedef bu::quantity<bu::si::area, double> Area;
typedef bu::quantity<bu::si::plane_angle, double> Angle;
}
......@@ -66,7 +66,8 @@ public:
// Output
std::atomic_bool calcInProgress;
QNemoProgress progress;
QNemoProgress qProgress;
std::vector<int> progress;
QNemoHeartbeat heartbeat;
QVector<QGeoCoordinate> waypoints;
......@@ -106,7 +107,7 @@ private:
SnakeDataManager::SnakeDataManager(QObject *parent)
: QThread(parent)
, _pImpl(std::make_unique<SnakeImpl>())
, pImpl(std::make_unique<SnakeImpl>())
{
}
......@@ -118,85 +119,85 @@ SnakeDataManager::~SnakeDataManager()
void SnakeDataManager::setMeasurementArea(const QList<QGeoCoordinate> &measurementArea)
{
UniqueLock lk(this->_pImpl->m);
this->_pImpl->mArea = measurementArea;
UniqueLock lk(this->pImpl->mutex);
this->pImpl->mArea = measurementArea;
}
void SnakeDataManager::setServiceArea(const QList<QGeoCoordinate> &serviceArea)
{
UniqueLock lk(this->_pImpl->m);
this->_pImpl->sArea = serviceArea;
UniqueLock lk(this->pImpl->mutex);
this->pImpl->sArea = serviceArea;
}
void SnakeDataManager::setCorridor(const QList<QGeoCoordinate> &corridor)
{
UniqueLock lk(this->_pImpl->m);
this->_pImpl->corridor = corridor;
UniqueLock lk(this->pImpl->mutex);
this->pImpl->corridor = corridor;
}
QNemoProgress SnakeDataManager::progress()
{
SharedLock lk(this->_pImpl->m);
return this->_pImpl->progress;
SharedLock lk(this->pImpl->mutex);
return this->pImpl->qProgress;
}
QNemoHeartbeat SnakeDataManager::heartbeat()
{
SharedLock lk(this->_pImpl->m);
return this->_pImpl->heartbeat;
SharedLock lk(this->pImpl->mutex);
return this->pImpl->heartbeat;
}
bool SnakeDataManager::calcInProgress()
{
return this->_pImpl->calcInProgress.load();
return this->pImpl->calcInProgress.load();
}
Length SnakeDataManager::lineDistance() const
{
SharedLock lk(this->_pImpl->m);
return this->_pImpl->lineDistance;
SharedLock lk(this->pImpl->mutex);
return this->pImpl->lineDistance;
}
void SnakeDataManager::setLineDistance(Length lineDistance)
{
UniqueLock lk(this->_pImpl->m);
this->_pImpl->lineDistance = lineDistance;
UniqueLock lk(this->pImpl->mutex);
this->pImpl->lineDistance = lineDistance;
}
Area SnakeDataManager::minTileArea() const
{
SharedLock lk(this->_pImpl->m);
return this->_pImpl->scenario.minTileArea();
SharedLock lk(this->pImpl->mutex);
return this->pImpl->scenario.minTileArea();
}
void SnakeDataManager::setMinTileArea(Area minTileArea)
{
UniqueLock lk(this->_pImpl->m);
this->_pImpl->scenario.setMinTileArea(minTileArea);
UniqueLock lk(this->pImpl->mutex);
this->pImpl->scenario.setMinTileArea(minTileArea);
}
Length SnakeDataManager::tileHeight() const
{
SharedLock lk(this->_pImpl->m);
return this->_pImpl->scenario.tileHeight();
SharedLock lk(this->pImpl->mutex);
return this->pImpl->scenario.tileHeight();
}
void SnakeDataManager::setTileHeight(Length tileHeight)
{
UniqueLock lk(this->_pImpl->m);
this->_pImpl->scenario.setTileHeight(tileHeight);
UniqueLock lk(this->pImpl->mutex);
this->pImpl->scenario.setTileHeight(tileHeight);
}
Length SnakeDataManager::tileWidth() const
{
SharedLock lk(this->_pImpl->m);
return this->_pImpl->scenario.tileWidth();
SharedLock lk(this->pImpl->mutex);
return this->pImpl->scenario.tileWidth();
}
void SnakeDataManager::setTileWidth(Length tileWidth)
{
UniqueLock lk(this->_pImpl->m);
this->_pImpl->scenario.setTileWidth(tileWidth);
UniqueLock lk(this->pImpl->mutex);
this->pImpl->scenario.setTileWidth(tileWidth);
}
bool SnakeDataManager::precondition() const
......@@ -204,81 +205,143 @@ bool SnakeDataManager::precondition() const
return true;
}
//!
//! \brief Resets waypoint data.
//! \pre this->_pImpl->mutex must be locked.
void SnakeDataManager::resetWaypointData()
{
this->pImpl->waypoints.clear();
this->pImpl->arrivalPath.clear();
this->pImpl->returnPath.clear();
this->pImpl->ENUorigin = QGeoCoordinate(0.0,0.0,0.0);
this->pImpl->waypointsENU.clear();
this->pImpl->arrivalPathENU.clear();
this->pImpl->returnPathENU.clear();
this->pImpl->tiles.polygons().clear();
this->pImpl->tileCenterPoints.clear();
this->pImpl->tilesENU.polygons().clear();
this->pImpl->tileCenterPointsENU.clear();
this->pImpl->errorMessage.clear();
}
void SnakeDataManager::run()
{
this->_pImpl->calcInProgress.store(true);
ToggleRAII<std::atomic_bool> tr(this->_pImpl->calcInProgress);
this->pImpl->calcInProgress.store(true);
ToggleRAII<std::atomic_bool> tr(this->pImpl->calcInProgress);
UniqueLock lk(&_pImpl->m);
_pImpl->clear();
UniqueLock lk(this->pImpl->mutex);
resetWaypointData();
if ( !precondition() )
return;
if ( this->_pImpl->mArea.size() < 3) {
_pImpl->errorMessage = "Measurement area invalid: size < 3.";
if ( this->pImpl->mArea.size() < 3) {
pImpl->errorMessage = "Measurement area invalid: size < 3.";
return;
}
if ( this->_pImpl->sArea.size() < 3) {
_pImpl->errorMessage = "Service area invalid: size < 3.";
if ( this->pImpl->sArea.size() < 3) {
pImpl->errorMessage = "Service area invalid: size < 3.";
return;
}
|| _sArea.size() <= 0
|| _corridor.size() <= 0 ){
auto origin = _mArea.front();
// Measurement area update necessary.
if ( _mArea.size() != _pScenario->measurementArea().outer().size() ){
{
QMutexLocker lk(&_pImpl->m);
_pImpl->ENUorigin = origin;
}
for (auto geoVertex : _mArea){
snake::BoostPoint p;
snake::toENU(origin, geoVertex, p);
_pScenario->measurementArea().outer().push_back(p);
}
this->pImpl->ENUorigin = this->pImpl->mArea.front();
auto &origin = this->pImpl->ENUorigin;
// Update measurement area.
auto &mAreaEnu = this->pImpl->scenario.measurementArea();
auto &mArea = this->pImpl->mArea;
mAreaEnu.clear();
for (auto geoVertex : mArea){
snake::BoostPoint p;
snake::toENU(origin, geoVertex, p);
mAreaEnu.outer().push_back(p);
}
// 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);
}
// Update service area.
auto &sAreaEnu = this->pImpl->scenario.serviceArea();
auto &sArea = this->pImpl->sArea;
sAreaEnu.clear();
for (auto geoVertex : sArea){
snake::BoostPoint p;
snake::toENU(origin, geoVertex, p);
sAreaEnu.outer().push_back(p);
}
// 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);
}
// Update corridor.
auto &corridorEnu = this->pImpl->scenario.corridor();
auto &corridor = this->pImpl->corridor;
corridor.clear();
for (auto geoVertex : corridor){
snake::BoostPoint p;
snake::toENU(origin, geoVertex, p);
corridorEnu.outer().push_back(p);
}
if ( !_pScenario->update() ){
{
QMutexLocker lk(&_pImpl->m);
for (auto c : _pScenario->errorString){
_pImpl->errorMessage.push_back(QChar(c));
}
}
if ( !this->pImpl->scenario.update() ){
pImpl->errorMessage = this->pImpl->scenario.errorString.c_str();
return;
}
// Asynchronously update flightplan.
auto future = std::async(std::bind(&snake::Flightplan::update, _pFlightplan));
qWarning() << "route calculation missing";
std::string errorString;
auto future = std::async([this, &errorString]{
auto alpha = -this->pImpl->scenario.mAreaBoundingBox().angle*degree::degree;
snake::flight_plan::Transects transects;
transects.push_back(bg::model::linestring{this->pImpl->scenario.homePositon()});
bool value = snake::flight_plan::transectsFromScenario(this->pImpl->lineDistance,
this->pImpl->minTransectLength,
alpha,
this->pImpl->scenario,
this->pImpl->progress,
transects,
errorString);
if ( !value )
return value;
snake::flight_plan::Transects transectsRouted;
snake::flight_plan::Route route;
value = snake::flight_plan::route(this->pImpl->scenario.joinedArea(),
transects,
transectsRouted,
route,
errorString);
if ( !value )
return value;
// Store waypoints.
for (auto boostVertex : _pFlightplan->waypoints()){
QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(origin, boostVertex, geoVertex);
pImpl->waypointsENU.push_back(enuVertex);
pImpl->waypoints.push_back(geoVertex);
}
// Store arrival path.
for (auto boostVertex : _pFlightplan->arrivalPath()){
QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(origin, boostVertex, geoVertex);
pImpl->arrivalPathENU.push_back(enuVertex);
pImpl->arrivalPath.push_back(geoVertex);
}
// Store return path.
for (auto boostVertex : _pFlightplan->returnPath()){
QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(origin, boostVertex, geoVertex);
pImpl->returnPathENU.push_back(enuVertex);
pImpl->returnPath.push_back(geoVertex);
}
return true;
});
// Continue with storing scenario data in the mean time.
{
QMutexLocker lk(&_pImpl->m);
// Get tiles.
const auto &tiles = _pScenario->tiles();
const auto &centerPoints = _pScenario->tileCenterPoints();
const auto &tiles = this->pImpl->scenario.tiles();
const auto &centerPoints = this->pImpl->scenario.tileCenterPoints();
for ( unsigned int i=0; i < tiles.size(); ++i ) {
const auto &tile = tiles[i];
SnakeTile geoTile;
......@@ -296,10 +359,10 @@ void SnakeDataManager::run()
QGeoCoordinate geoVertex;
snake::fromENU(origin, boostPoint, geoVertex);
geoTile.setCenter(geoVertex);
_pImpl->tiles.polygons().push_back(geoTile);
_pImpl->tileCenterPoints.push_back(QVariant::fromValue(geoVertex));
_pImpl->tilesENU.polygons().push_back(enuTile);
_pImpl->tileCenterPointsENU.push_back(enuVertex);
pImpl->tiles.polygons().push_back(geoTile);
pImpl->tileCenterPoints.push_back(QVariant::fromValue(geoVertex));
pImpl->tilesENU.polygons().push_back(enuTile);
pImpl->tileCenterPointsENU.push_back(enuVertex);
}
}
......@@ -307,37 +370,12 @@ void SnakeDataManager::run()
// Trying to generate flight plan.
if ( !future.get() ){
// error
QMutexLocker lk(&_pImpl->m);
for (auto c : _pFlightplan->errorString){
_pImpl->errorMessage.push_back(QChar(c));
pImpl->errorMessage.push_back(QChar(c));
}
} else {
//success!!!
QMutexLocker lk(&_pImpl->m);
// Store waypoints.
for (auto boostVertex : _pFlightplan->waypoints()){
QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(origin, boostVertex, geoVertex);
_pImpl->waypointsENU.push_back(enuVertex);
_pImpl->waypoints.push_back(geoVertex);
}
// Store arrival path.
for (auto boostVertex : _pFlightplan->arrivalPath()){
QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(origin, boostVertex, geoVertex);
_pImpl->arrivalPathENU.push_back(enuVertex);
_pImpl->arrivalPath.push_back(geoVertex);
}
// Store return path.
for (auto boostVertex : _pFlightplan->returnPath()){
QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
QGeoCoordinate geoVertex;
snake::fromENU(origin, boostVertex, geoVertex);
_pImpl->returnPathENU.push_back(enuVertex);
_pImpl->returnPath.push_back(geoVertex);
}
// Success!!!
}
}
......
......@@ -57,7 +57,8 @@ protected:
private:
bool precondition() const;
SnakeImplPtr _pImpl;
void resetWaypointData();
SnakeImplPtr pImpl;
};
......
......@@ -200,6 +200,18 @@ Fact *WimaController::altitude() {
return &_altitude;
}
QmlObjectListModel *WimaController::snakeTiles()
{
qWarning() << "using snake tile dummy";
return QmlObjectListModel();
}
QVariantList WimaController::snakeTileCenterPoints()
{
qWarning() << "using snakeTileCenterPoints dummy";
return QVariantList();
}
QVector<int> WimaController::nemoProgress() {
if ( _nemoProgress.progress().size() == _snakeTileCenterPoints.size() )
return _nemoProgress.progress();
......
......@@ -216,8 +216,8 @@ public:
Fact* snakeLineDistance (void) { return &_snakeLineDistance;}
Fact* snakeMinTransectLength (void) { return &_snakeMinTransectLength;}
// Snake data.
QmlObjectListModel* snakeTiles (void) { return _snakeTiles.QmlObjectListModel();}
QVariantList snakeTileCenterPoints (void) { return _snakeTileCenterPoints;}
QmlObjectListModel* snakeTiles (void);
QVariantList snakeTileCenterPoints (void);
QVector<int> nemoProgress (void);
int nemoStatus (void) const;
QString nemoStatusString (void) const;
......
Markdown is supported
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