Skip to content

Q
qgroundcontrol
Commit 1ac83998 authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files
Options

wima planer, cs and cs worker edited

parent 46adaf36
Hide whitespace changes
Inline Side-by-side
Showing with 899 additions and 821 deletions
Paths/KlingenbachTest.wima
View file @ 1ac83998
......@@ -3,6 +3,9 @@
{
"AreaType": "Service Area",
"BorderPolygonOffset": 6,
"DepotAltitude": 0,
"DepotLatitude": 47.76798743982897,
"DepotLongitude": 16.529971617189517,
"ShowBorderPolygon": 0,
"maxAltitude": 30,
"polygon": [
......@@ -57,11 +60,11 @@
16.530403347547246
],
[
47.76800516387783,
47.76800516387784,
16.530513932174728
],
[
47.767998146781224,
47.76799814678123,
16.530388491662848
]
]
......@@ -92,8 +95,8 @@
0,
0,
null,
47.76779208273835,
16.53052025186404,
47.76794255683365,
16.53029214577345,
5
],
"type": "SimpleItem"
......@@ -129,8 +132,8 @@
0,
0,
null,
47.76818883948184,
16.531059344799864,
47.76786965695791,
16.5304471058032,
15
],
"type": "SimpleItem"
......@@ -145,8 +148,8 @@
0,
0,
null,
47.76819200424394,
16.53094084541531,
47.767852121581285,
16.530624995372722,
15
],
"type": "SimpleItem"
......@@ -161,8 +164,8 @@
0,
0,
null,
47.76820508527868,
16.5307314416142,
47.768280070477275,
16.531012421511814,
15
],
"type": "SimpleItem"
......@@ -177,8 +180,8 @@
0,
0,
null,
47.7682219391165,
16.530560469333935,
47.76828174772784,
16.53094964311624,
15
],
"type": "SimpleItem"
......@@ -193,8 +196,8 @@
0,
0,
null,
47.76815219351091,
16.530388084190125,
47.768293889869426,
16.530755276539708,
15
],
"type": "SimpleItem"
......@@ -209,8 +212,8 @@
0,
0,
null,
47.768136979184064,
16.530500573736578,
47.7682219391165,
16.530560469333935,
15
],
"type": "SimpleItem"
......@@ -225,8 +228,8 @@
0,
0,
null,
47.76811577885341,
16.530715631144773,
47.76820508527868,
16.5307314416142,
15
],
"type": "SimpleItem"
......@@ -241,8 +244,8 @@
0,
0,
null,
47.76810225985858,
16.5309320464106,
47.76819200424394,
16.53094084541531,
15
],
"type": "SimpleItem"
......@@ -257,8 +260,8 @@
0,
0,
null,
47.76809760666698,
16.53110626925785,
47.76818883948184,
16.531059344799864,
15
],
"type": "SimpleItem"
......@@ -273,8 +276,8 @@
0,
0,
null,
47.76800654291486,
16.531146891465884,
47.76809760666698,
16.53110626925785,
15
],
"type": "SimpleItem"
......@@ -289,8 +292,8 @@
0,
0,
null,
47.768012516370526,
16.530923248770208,
47.76810225985858,
16.5309320464106,
15
],
"type": "SimpleItem"
......@@ -305,8 +308,8 @@
0,
0,
null,
47.76802647332397,
16.530699820729467,
47.76811577885341,
16.530715631144773,
15
],
"type": "SimpleItem"
......@@ -321,8 +324,8 @@
0,
0,
null,
47.76804836071953,
16.53047779455375,
47.768136979184064,
16.530500573736578,
15
],
"type": "SimpleItem"
......@@ -337,8 +340,8 @@
0,
0,
null,
47.768077491470365,
16.530262415675615,
47.7681521944103,
16.530388084190108,
15
],
"type": "SimpleItem"
......@@ -353,8 +356,8 @@
0,
0,
null,
47.76797480993848,
16.5303436148631,
47.76807542298544,
16.53027770936353,
15
],
"type": "SimpleItem"
......@@ -369,8 +372,8 @@
0,
0,
null,
47.76795974224912,
16.53045501678237,
47.76804836071953,
16.53047779455375,
15
],
"type": "SimpleItem"
......@@ -385,8 +388,8 @@
0,
0,
null,
47.76793716779096,
16.530684010368297,
47.76802647332397,
16.530699820729467,
15
],
"type": "SimpleItem"
......@@ -401,8 +404,8 @@
0,
0,
null,
47.76792538641036,
16.530872608510265,
47.768012516370526,
16.530923248770208,
15
],
"type": "SimpleItem"
......@@ -417,8 +420,8 @@
0,
0,
null,
47.76785212158128,
16.530624994038668,
47.76800654291486,
16.531146891465884,
15
],
"type": "SimpleItem"
......@@ -433,8 +436,8 @@
0,
0,
null,
47.76787112287341,
16.530432237754336,
47.76792538641036,
16.530872608510265,
15
],
"type": "SimpleItem"
......@@ -449,8 +452,24 @@
0,
0,
null,
47.76787892292753,
16.53037457049453,
47.76793716779096,
16.530684010368297,
15
],
"type": "SimpleItem"
},
{
"autoContinue": true,
"command": 16,
"doJumpId": 23,
"frame": 3,
"params": [
0,
0,
0,
null,
47.767956224940995,
16.530490697489185,
15
],
"type": "SimpleItem"
......@@ -460,88 +479,92 @@
"TurnAroundDistance": 10,
"VisualTransectPoints": [
[
47.76818883948184,
16.531059344799864
47.76786965695791,
16.5304471058032
],
[
47.76819200424394,
16.53094084541531
47.767852121581285,
16.530624995372722
],
[
47.76820508527868,
16.5307314416142
47.768280070477275,
16.531012421511814
],
[
47.76828174772784,
16.53094964311624
],
[
47.768293889869426,
16.530755276539708
],
[
47.7682219391165,
16.530560469333935
],
[
47.76815219351091,
16.530388084190125
47.76820508527868,
16.5307314416142
],
[
47.768136979184064,
16.530500573736578
47.76819200424394,
16.53094084541531
],
[
47.76811577885341,
16.530715631144773
47.76818883948184,
16.531059344799864
],
[
47.76809760666698,
16.53110626925785
],
[
47.76810225985858,
16.5309320464106
],
[
47.76809760666698,
16.53110626925785
47.76811577885341,
16.530715631144773
],
[
47.76800654291486,
16.531146891465884
47.768136979184064,
16.530500573736578
],
[
47.768012516370526,
16.530923248770208
47.7681521944103,
16.530388084190108
],
[
47.76802647332397,
16.530699820729467
47.76807542298544,
16.53027770936353
],
[
47.76804836071953,
16.53047779455375
],
[
47.768077491470365,
16.530262415675615
],
[
47.76797480993848,
16.5303436148631
47.76802647332397,
16.530699820729467
],
[
47.76795974224912,
16.53045501678237
47.768012516370526,
16.530923248770208
],
[
47.76793716779096,
16.530684010368297
47.76800654291486,
16.531146891465884
],
[
47.76792538641036,
16.530872608510265
],
[
47.76785212158128,
16.530624994038668
],
[
47.76787112287341,
16.530432237754336
47.76793716779096,
16.530684010368297
],
[
47.76787892292753,
16.53037457049453
47.767956224940995,
16.530490697489185
]
],
"version": 1
......@@ -569,17 +592,40 @@
16.530403347547246
],
[
47.76797785674764,
16.530341265362296
47.76800516387783,
16.530513932174728
],
[
47.767998146781224,
16.530388491662848
]
],
"type": "ComplexItem",
"version": 1
},
{
"AMSLAltAboveTerrain": null,
"Altitude": 0,
"AltitudeMode": 1,
"autoContinue": true,
"command": 21,
"doJumpId": 27,
"frame": 3,
"params": [
0,
0,
0,
null,
47.76794255683365,
16.53029214577345,
0
],
"type": "SimpleItem"
}
],
"plannedHomePosition": [
47.76779208273835,
16.530520251864044,
47.76794255683365,
16.53029214577345,
178
],
"vehicleType": 2,
......
src/FlightDisplay/FlightDisplayViewMap.qml
View file @ 1ac83998
......@@ -241,7 +241,7 @@ FlightMap {
color: "green"
}
// Add Snake tiles center points to the map
// Add Snake tile center points to the map
MapItemView {
property bool _enable: wimaController.enableWimaController.value
&& wimaController.enableSnake.value
......@@ -249,7 +249,7 @@ FlightMap {
delegate: MapCircle{
center: modelData
border.color: "black"
border.color: "transparent"
border.width: 1
color: getColor(wimaController.nemoProgress[index])
radius: 0.6
......@@ -258,7 +258,7 @@ FlightMap {
function getColor(progress) {
if (progress < 50)
return "orangered"
return "red"
if (progress < 100)
return "orange"
......
src/Wima/CSWorker.cpp
View file @ 1ac83998
#include "CSWorker.h"
// Wima
#define CLIPPER_SCALE 10000
#include "clipper/clipper.hpp"
template <int k> ClipperLib::cInt get(ClipperLib::IntPoint &p);
#include "Geometry/GenericCircle.h"
// std
#include <chrono>
// Qt
#include <QDebug>
template <> ClipperLib::cInt get<0>(ClipperLib::IntPoint &p) { return p.X; }
template <> ClipperLib::cInt get<1>(ClipperLib::IntPoint &p) { return p.Y; }
RoutingWorker::RoutingWorker(QObject *parent)
: QThread(parent), _calculating(false), _stop(false), _restart(false) {
CSWorker::CSWorker(QObject *parent)
: QThread(parent), _deltaR(2 * bu::si::meter),
_deltaAlpha(3 * bu::degree::degree), _minLength(10 * bu::si::meter),
_useDepotSafeArea(false), _calculating(false), _stop(false),
_restart(false) {}
static std::once_flag flag;
std::call_once(flag,
[] { qRegisterMetaType<PtrRoutingData>("PtrRoutingData"); });
}
CSWorker::~CSWorker() {
RoutingWorker::~RoutingWorker() {
this->_stop = true;
Lock lk(this->_mutex);
this->_restart = true;
......@@ -27,57 +21,15 @@ CSWorker::~CSWorker() {
this->wait();
}
bool CSWorker::calculating() { return this->_calculating; }
void CSWorker::update(const QList<QGeoCoordinate> &polygon,
const QGeoCoordinate &origin, snake::Length deltaR,
snake::Length minLength, snake::Angle deltaAlpha) {
// Sample input.
Lock lk(this->_mutex);
this->_polygon = polygon;
for (auto &v : this->_polygon) {
v.setAltitude(0);
}
this->_origin = origin;
this->_origin.setAltitude(0);
this->_deltaR = deltaR;
this->_deltaAlpha = deltaAlpha;
this->_minLength = minLength;
lk.unlock();
this->_useDepotSafeArea = false;
if (!this->isRunning()) {
this->start();
} else {
Lock lk(this->_mutex);
this->_restart = true;
this->_cv.notify_one();
}
}
bool RoutingWorker::calculating() { return this->_calculating; }
void CSWorker::update(const QGeoCoordinate &depot,
const QList<QGeoCoordinate> &safeArea,
const QList<QGeoCoordinate> &polygon,
const QGeoCoordinate &origin, snake::Length deltaR,
snake::Length minLength, snake::Angle deltaAlpha) {
void RoutingWorker::route(const snake::BoostPolygon &safeArea,
const RoutingWorker::Generator &generator) {
// Sample input.
Lock lk(this->_mutex);
this->_depot = depot;
this->_safeArea = safeArea;
for (auto &v : this->_safeArea) {
v.setAltitude(0);
}
this->_polygon = polygon;
for (auto &v : this->_polygon) {
v.setAltitude(0);
}
this->_origin = origin;
this->_origin.setAltitude(0);
this->_deltaR = deltaR;
this->_deltaAlpha = deltaAlpha;
this->_minLength = minLength;
this->_generator = generator;
lk.unlock();
this->_useDepotSafeArea = true;
if (!this->isRunning()) {
this->start();
......@@ -88,363 +40,90 @@ void CSWorker::update(const QGeoCoordinate &depot,
}
}
void CSWorker::run() {
qWarning() << "CSWorker::run(): thread start.";
void RoutingWorker::run() {
qWarning() << "RoutingWorker::run(): thread start.";
while (!this->_stop) {
// Copy input.
QGeoCoordinate depot;
QList<QGeoCoordinate> safeArea;
Lock lk(this->_mutex);
if (this->_useDepotSafeArea) {
depot = this->_depot;
safeArea = this->_safeArea;
} else {
depot = this->_origin;
safeArea = this->_polygon;
}
const auto polygon = this->_polygon;
const auto origin = this->_origin;
const auto deltaR = this->_deltaR;
const auto deltaAlpha = this->_deltaAlpha;
const auto minLength = this->_minLength;
lk.unlock();
// Check preconitions
if (polygon.size() >= 3) {
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning() << "CSWorker::run(): calculation "
"started.";
qWarning() << "RoutingWorker::run(): calculation "
"started.";
#endif
// Copy input.
#ifdef SHOW_CIRCULAR_SURVEY_TIME
const auto start = std::chrono::high_resolution_clock::now();
#endif
using namespace boost::units;
this->_calculating = true;
emit calculatingChanged();
// Convert geo polygon to ENU polygon.
snake::BoostPolygon polygonENU;
snake::BoostPolygon safeAreaENU;
snake::BoostPoint originENU{0, 0};
snake::BoostPoint depotENU;
snake::areaToEnu(origin, polygon, polygonENU);
snake::areaToEnu(origin, safeArea, safeAreaENU);
snake::toENU(origin, depot, depotENU);
std::string error;
// Check validity.
if (!bg::is_valid(polygonENU, error) ||
!bg::is_valid(safeAreaENU, error)) {
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning() << "CSWorker::run(): "
"invalid polygon.";
qWarning() << error.c_str();
auto start = std::chrono::high_resolution_clock::now();
#endif
} else {
// Calculate polygon distances and angles.
std::vector<snake::Length> distances;
distances.reserve(polygonENU.outer().size());
std::vector<snake::Angle> angles;
angles.reserve(polygonENU.outer().size());
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning() << "CSWorker::run():";
#endif
for (const auto &p : polygonENU.outer()) {
snake::Length distance = bg::distance(originENU, p) * si::meter;
distances.push_back(distance);
snake::Angle alpha =
(std::atan2(p.get<1>(), p.get<0>())) * si::radian;
alpha =
alpha < 0 * si::radian ? alpha + 2 * M_PI * si::radian : alpha;
angles.push_back(alpha);
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning() << "distances, angles, coordinates:";
qWarning() << to_string(distance).c_str();
qWarning() << to_string(snake::Degree(alpha)).c_str();
qWarning() << "x = " << p.get<0>() << "y = " << p.get<1>();
#endif
}
auto rMin = deltaR; // minimal circle radius
snake::Angle alpha1(0 * degree::degree);
snake::Angle alpha2(360 * degree::degree);
// Determine r_min by successive approximation
if (!bg::within(originENU, polygonENU)) {
rMin = bg::distance(originENU, polygonENU) * si::meter;
}
auto rMax =
(*std::max_element(distances.begin(),
distances.end())); // maximal circle radius
// Scale parameters and coordinates.
const auto rMinScaled =
ClipperLib::cInt(std::round(rMin.value() * CLIPPER_SCALE));
const auto deltaRScaled =
ClipperLib::cInt(std::round(deltaR.value() * CLIPPER_SCALE));
auto originScaled = ClipperLib::IntPoint{
ClipperLib::cInt(std::round(originENU.get<0>())),
ClipperLib::cInt(std::round(originENU.get<1>()))};
this->_calculating = true;
emit calculatingChanged();
Lock lk(this->_mutex);
auto safeAreaENU = this->_safeArea;
auto generator = this->_generator;
lk.unlock();
#ifdef SHOW_CIRCULAR_SURVEY_TIME
auto s1 = std::chrono::high_resolution_clock::now();
#endif
// Generate circle sectors.
auto rScaled = rMinScaled;
const auto nTran = long(std::ceil(((rMax - rMin) / deltaR).value()));
vector<ClipperLib::Path> sectors(nTran, ClipperLib::Path());
const auto nSectors =
long(std::round(((alpha2 - alpha1) / deltaAlpha).value()));
PtrRoutingData pRouteData(new RoutingData());
auto &transectsENU = pRouteData->transects;
// Generate transects.
if (generator(transectsENU)) {
// Check if generation was successful.
if (transectsENU.size() == 0) {
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning() << "CSWorker::run(): sector parameres:";
qWarning() << "alpha1: " << to_string(snake::Degree(alpha1)).c_str();
qWarning() << "alpha2: " << to_string(snake::Degree(alpha2)).c_str();
qWarning() << "n: "
<< to_string((alpha2 - alpha1) / deltaAlpha).c_str();
qWarning() << "nSectors: " << nSectors;
qWarning() << "rMin: " << to_string(rMin).c_str();
qWarning() << "rMax: " << to_string(rMax).c_str();
qWarning() << "nTran: " << nTran;
#endif
using ClipperCircle =
GenericCircle<ClipperLib::cInt, ClipperLib::IntPoint>;
for (auto &sector : sectors) {
ClipperCircle circle(rScaled, originScaled);
approximate(circle, nSectors, sector);
rScaled += deltaRScaled;
}
// Clip sectors to polygonENU.
ClipperLib::Path polygonClipper;
snake::BoostPolygon shrinked;
snake::offsetPolygon(polygonENU, shrinked, -0.1);
auto &outer = shrinked.outer();
polygonClipper.reserve(outer.size() - 1);
for (auto it = outer.begin(); it < outer.end() - 1; ++it) {
auto x = ClipperLib::cInt(std::round(it->get<0>() * CLIPPER_SCALE));
auto y = ClipperLib::cInt(std::round(it->get<1>() * CLIPPER_SCALE));
polygonClipper.push_back(ClipperLib::IntPoint{x, y});
}
ClipperLib::Clipper clipper;
clipper.AddPath(polygonClipper, ClipperLib::ptClip, true);
clipper.AddPaths(sectors, ClipperLib::ptSubject, false);
ClipperLib::PolyTree transectsClipper;
clipper.Execute(ClipperLib::ctIntersection, transectsClipper,
ClipperLib::pftNonZero, ClipperLib::pftNonZero);
// Extract transects from PolyTree and convert them to
// BoostLineString
snake::Transects transectsENU;
if (this->_useDepotSafeArea) {
transectsENU.push_back(snake::BoostLineString{depotENU});
}
for (const auto &child : transectsClipper.Childs) {
snake::BoostLineString transect;
transect.reserve(child->Contour.size());
for (const auto &vertex : child->Contour) {
auto x = static_cast<double>(vertex.X) / CLIPPER_SCALE;
auto y = static_cast<double>(vertex.Y) / CLIPPER_SCALE;
transect.push_back(snake::BoostPoint(x, y));
}
transectsENU.push_back(transect);
}
// Join sectors which where slit due to clipping.
const double th = 0.01;
for (auto ito = transectsENU.begin(); ito < transectsENU.end(); ++ito) {
for (auto iti = ito + 1; iti < transectsENU.end(); ++iti) {
auto dist1 = bg::distance(ito->front(), iti->front());
if (dist1 < th) {
snake::BoostLineString temp;
for (auto it = iti->end() - 1; it >= iti->begin(); --it) {
temp.push_back(*it);
}
temp.insert(temp.end(), ito->begin(), ito->end());
*ito = temp;
transectsENU.erase(iti);
break;
}
auto dist2 = bg::distance(ito->front(), iti->back());
if (dist2 < th) {
snake::BoostLineString temp;
temp.insert(temp.end(), iti->begin(), iti->end());
temp.insert(temp.end(), ito->begin(), ito->end());
*ito = temp;
transectsENU.erase(iti);
break;
}
auto dist3 = bg::distance(ito->back(), iti->front());
if (dist3 < th) {
snake::BoostLineString temp;
temp.insert(temp.end(), ito->begin(), ito->end());
temp.insert(temp.end(), iti->begin(), iti->end());
*ito = temp;
transectsENU.erase(iti);
break;
}
auto dist4 = bg::distance(ito->back(), iti->back());
if (dist4 < th) {
snake::BoostLineString temp;
temp.insert(temp.end(), ito->begin(), ito->end());
for (auto it = iti->end() - 1; it >= iti->begin(); --it) {
temp.push_back(*it);
}
*ito = temp;
transectsENU.erase(iti);
break;
}
}
}
// Remove short transects
auto begin = this->_useDepotSafeArea ? transectsENU.begin() + 1
: transectsENU.begin();
for (auto it = begin; it < transectsENU.end();) {
if (bg::length(*it) < minLength.value()) {
it = transectsENU.erase(it);
} else {
++it;
}
}
if (!this->_useDepotSafeArea) {
// Move transect with min. distance to the front.
auto minDist = std::numeric_limits<double>::max();
auto minIt = transectsENU.begin();
bool reverse = false;
for (auto it = transectsENU.begin(); it < transectsENU.end(); ++it) {
auto distFront = bg::distance(originENU, it->front());
auto distBack = bg::distance(originENU, it->back());
if (distFront < minDist) {
minDist = distFront;
minIt = it;
reverse = false;
}
if (distBack < minDist) {
minDist = distBack;
minIt = it;
reverse = true;
}
}
// Swap and reverse (if necessary).
if (minIt != transectsENU.begin()) {
auto minTransect = *minIt;
if (reverse) {
snake::BoostLineString rev;
for (auto it = minTransect.end() - 1; it >= minTransect.begin();
--it) {
rev.push_back(*it);
}
minTransect = rev;
}
*minIt = *transectsENU.begin();
*transectsENU.begin() = minTransect;
}
}
#ifdef SHOW_CIRCULAR_SURVEY_TIME
qWarning() << "CSWorker::run(): transect gen. time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - s1)
.count()
<< " ms";
qWarning() << "RoutingWorker::run(): "
"not able to generate transects.";
#endif
if (transectsENU.size() == 0) {
} else {
// Prepare data for routing.
auto &transectsInfo = pRouteData->transectsInfo;
auto &route = pRouteData->route;
const auto routingStart = std::chrono::high_resolution_clock::now();
const auto maxRoutingTime = std::chrono::minutes(1);
const auto routingEnd = routingStart + maxRoutingTime;
const auto &restart = this->_restart;
auto stopLambda = [&restart, routingEnd] {
bool expired = std::chrono::high_resolution_clock::now() > routingEnd;
return restart || expired;
};
std::string errorString;
// Route transects;
bool success = snake::route(safeAreaENU, transectsENU, transectsInfo,
route, stopLambda, errorString);
// Check if routing was successful.
if ((!success || route.size() < 3) && !this->_restart) {
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning() << "CSWorker::run(): "
"not able to generate transects.";
qWarning() << "RoutingWorker::run(): "
"routing failed.";
#endif
} else if (this->_restart) {
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning() << "CSWorker::run(): "
qWarning() << "RoutingWorker::run(): "
"restart requested.";
#endif
} else {
// Prepare data for routing.
std::vector<snake::TransectInfo> transectsInfo;
snake::Route route;
const auto routingStart = std::chrono::high_resolution_clock::now();
const auto maxRoutingTime = std::chrono::minutes(1);
const auto routingEnd = routingStart + maxRoutingTime;
const auto &restart = this->_restart;
auto stopLambda = [&restart, routingEnd] {
bool expired =
std::chrono::high_resolution_clock::now() > routingEnd;
return restart || expired;
};
std::string errorString;
// Route transects;
bool success = snake::route(safeAreaENU, transectsENU, transectsInfo,
route, stopLambda, errorString);
if ((!success || route.size() < 3) && !this->_restart) {
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning() << "CSWorker::run(): "
"routing failed.";
#endif
} else if (this->_restart) {
// Notify main thread.
emit result(pRouteData);
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning() << "CSWorker::run(): "
"restart requested.";
qWarning() << "RoutingWorker::run(): "
"concurrent update success.";
#endif
} else {
// Find index of first waypoint.
std::size_t idxFirst = 0;
const auto &infoFirst = transectsInfo.front();
const auto &firstTransect = transectsENU[infoFirst.index];
const auto &firstWaypoint = infoFirst.reversed
? firstTransect.back()
: firstTransect.front();
double th = 0.001;
for (std::size_t i = 0; i < route.size(); ++i) {
auto dist = bg::distance(route[i], firstWaypoint);
if (dist < th) {
idxFirst = i;
break;
}
}
// Find index of last waypoint.
std::size_t idxLast = route.size() - 1;
const auto &infoLast = transectsInfo.back();
const auto &lastTransect = transectsENU[infoLast.index];
const auto &lastWaypoint =
infoLast.reversed ? lastTransect.front() : lastTransect.back();
for (long i = route.size() - 1; i >= 0; --i) {
auto dist = bg::distance(route[i], lastWaypoint);
if (dist < th) {
idxLast = i;
break;
}
}
// Convert to geo coordinates and notify main thread.
auto pRoute = PtrRoute(new Route());
for (std::size_t i = idxFirst; i <= idxLast; ++i) {
auto vertex = route[i];
QGeoCoordinate c;
snake::fromENU(origin, vertex, c);
pRoute->append(c);
}
emit ready(pRoute);
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning() << "CSWorker::run(): "
"concurrent update success.";
#endif
}
}
}
#ifdef SHOW_CIRCULAR_SURVEY_TIME
qWarning() << "CSWorker::run(): execution time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start)
.count()
<< " ms";
#endif
this->_calculating = false;
emit calculatingChanged();
}
} // end calculation
#ifdef DEBUG_CIRCULAR_SURVEY
else {
qWarning() << "CSWorker::run(): preconditions failed.";
qWarning() << "RoutingWorker::run(): preconditions failed.";
}
#endif
#ifdef SHOW_CIRCULAR_SURVEY_TIME
qWarning() << "RoutingWorker::run(): execution time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start)
.count()
<< " ms";
#endif
this->_calculating = false;
emit calculatingChanged();
Lock lk2(this->_mutex);
if (!this->_restart) {
this->_cv.wait(lk2, [this] { return this->_restart.load(); });
}
this->_restart = false;
}
qWarning() << "CSWorker::run(): thread end.";
} // main loop
qWarning() << "RoutingWorker::run(): thread end.";
}
src/Wima/CSWorker.h
View file @ 1ac83998
#pragma once
#include <QGeoCoordinate>
#include <QList>
#include <QSharedPointer>
#include <QThread>
#include "snake.h"
#include <atomic>
#include <condition_variable>
#include <functional>
#include <mutex>
struct RoutingData {
snake::BoostLineString route;
snake::Transects transects;
std::vector<snake::TransectInfo> transectsInfo;
};
//!
//! \brief The CSWorker class
//! \note Don't call QThread::start, QThread::quit, etc. onyl use Worker
//! members!
class CSWorker : public QThread {
class RoutingWorker : public QThread {
Q_OBJECT
using Lock = std::unique_lock<std::mutex>;
public:
using Route = QList<QGeoCoordinate>;
using PtrRoute = QSharedPointer<Route>;
using PtrRoutingData = QSharedPointer<RoutingData>;
using Generator = std::function<bool(snake::Transects &)>;
CSWorker(QObject *parent = nullptr);
~CSWorker() override;
RoutingWorker(QObject *parent = nullptr);
~RoutingWorker() override;
bool calculating();
public slots:
void update(const QList<QGeoCoordinate> &polygon,
const QGeoCoordinate &origin, snake::Length deltaR,
snake::Length minLength, snake::Angle deltaAlpha);
void update(const QGeoCoordinate &depot,
const QList<QGeoCoordinate> &safeArea,
const QList<QGeoCoordinate> &polygon,
const QGeoCoordinate &origin, snake::Length deltaR,
snake::Length minLength, snake::Angle deltaAlpha);
void route(const snake::BoostPolygon &safeArea, const Generator &generator);
signals:
void ready(PtrRoute pTransects);
void result(PtrRoutingData pTransects);
void calculatingChanged();
protected:
......@@ -48,17 +47,9 @@ private:
mutable std::mutex _mutex;
mutable std::condition_variable _cv;
// Internal data
QGeoCoordinate _depot;
QList<QGeoCoordinate> _safeArea;
QList<QGeoCoordinate> _polygon;
QGeoCoordinate _origin;
snake::Length _deltaR;
snake::Angle _deltaAlpha;
snake::Length _minLength;
std::size_t _maxWaypoints;
snake::BoostPolygon _safeArea;
Generator _generator; // transect generator
// State
std::atomic_bool _useDepotSafeArea;
std::atomic_bool _calculating;
std::atomic_bool _stop;
std::atomic_bool _restart;
......
src/Wima/CircularSurvey.cc
View file @ 1ac83998
......@@ -5,6 +5,13 @@
#include "QGCApplication.h"
// Wima
#include "snake.h"
#define CLIPPER_SCALE 10000
#include "clipper/clipper.hpp"
template <int k> ClipperLib::cInt get(ClipperLib::IntPoint &p);
template <> ClipperLib::cInt get<0>(ClipperLib::IntPoint &p) { return p.X; }
template <> ClipperLib::cInt get<1>(ClipperLib::IntPoint &p) { return p.Y; }
#include "Geometry/GenericCircle.h"
// boost
#include <boost/units/io.hpp>
#include <boost/units/systems/si.hpp>
......@@ -18,6 +25,11 @@ private:
Functor fun;
};
bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot,
const QList<QGeoCoordinate> &polygon,
snake::Length deltaR, snake::Angle deltaAlpha,
snake::Length minLength, snake::Transects &transects);
const char *CircularSurvey::settingsGroup = "CircularSurvey";
const char *CircularSurvey::deltaRName = "DeltaR";
const char *CircularSurvey::deltaAlphaName = "DeltaAlpha";
......@@ -36,8 +48,8 @@ CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView,
_deltaR(settingsGroup, _metaDataMap[deltaRName]),
_deltaAlpha(settingsGroup, _metaDataMap[deltaAlphaName]),
_minLength(settingsGroup, _metaDataMap[transectMinLengthName]),
_pWorker(std::make_unique<CSWorker>()), _needsStoring(false),
_needsReversal(false) {
_pWorker(std::make_unique<RoutingWorker>()), _needsStoring(false),
_needsReversal(false), _hidePolygon(false) {
Q_UNUSED(kmlOrShpFile)
_editorQml = "qrc:/qml/CircularSurveyItemEditor.qml";
......@@ -55,10 +67,9 @@ CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView,
connect(this, &CircularSurvey::safeAreaChanged, this,
&CircularSurvey::_rebuildTransects);
// Connect worker.
qRegisterMetaType<PtrRoute>("PtrRoute");
connect(this->_pWorker.get(), &CSWorker::ready, this,
connect(this->_pWorker.get(), &RoutingWorker::result, this,
&CircularSurvey::_setTransects);
connect(this->_pWorker.get(), &CSWorker::calculatingChanged, this,
connect(this->_pWorker.get(), &RoutingWorker::calculatingChanged, this,
&CircularSurvey::calculatingChanged);
this->_transectsDirty = false;
}
......@@ -310,7 +321,11 @@ bool CircularSurvey::readyForSave() const {
double CircularSurvey::additionalTimeDelay() const { return 0; }
void CircularSurvey::_rebuildTransectsPhase1(void) {
// Store result of former calculation.
if (this->_needsStoring) {
#ifdef SHOW_CIRCULAR_SURVEY_TIME
auto start = std::chrono::high_resolution_clock::now();
#endif
// If the transects are getting rebuilt then any previously loaded
// mission items are now invalid.
if (_loadedMissionItemsParent) {
......@@ -318,15 +333,71 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
_loadedMissionItemsParent->deleteLater();
_loadedMissionItemsParent = nullptr;
}
// Copy Transects.
// Store raw transects.
this->_rawTransects.clear();
const auto &transectsENU = this->_workerOutput->transects;
const auto &ori = this->_referencePoint;
for (auto &t : transectsENU) {
QList<QGeoCoordinate> trGeo;
for (auto &v : t) {
QGeoCoordinate c;
snake::fromENU(ori, v, c);
trGeo.append(c);
}
this->_rawTransects.append(trGeo);
}
// Store route.
const auto &transectsInfo = this->_workerOutput->transectsInfo;
const auto &route = this->_workerOutput->route;
// Find index of first waypoint.
std::size_t idxFirst = 0;
const auto &infoFirst = transectsInfo.front();
const auto &firstTransect = transectsENU[infoFirst.index];
const auto &firstWaypoint =
infoFirst.reversed ? firstTransect.back() : firstTransect.front();
double th = 0.001;
for (std::size_t i = 0; i < route.size(); ++i) {
auto dist = bg::distance(route[i], firstWaypoint);
if (dist < th) {
idxFirst = i;
break;
}
}
// Find index of last waypoint.
std::size_t idxLast = route.size() - 1;
const auto &infoLast = transectsInfo.back();
const auto &lastTransect = transectsENU[infoLast.index];
const auto &lastWaypoint =
infoLast.reversed ? lastTransect.front() : lastTransect.back();
for (long i = route.size() - 1; i >= 0; --i) {
auto dist = bg::distance(route[i], lastWaypoint);
if (dist < th) {
idxLast = i;
break;
}
}
// Convert to geo coordinates.
QList<CoordInfo_t> list;
for (const auto c : *this->_pRoute) {
for (std::size_t i = idxFirst; i <= idxLast; ++i) {
auto vertex = route[i];
QGeoCoordinate c;
snake::fromENU(ori, vertex, c);
list.append(CoordInfo_t{c, CoordTypeInterior});
}
this->_transects.append(list);
// Mark transect as stored;
// Mark transect as stored and ready.
this->_needsStoring = false;
} else if (this->_needsReversal) {
this->_transectsDirty = false;
#ifdef SHOW_CIRCULAR_SURVEY_TIME
qWarning() << "CS::rebuildTransectsPhase1(): store: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start)
.count()
<< " ms";
#endif
}
// Reverse transects only.
else if (this->_needsReversal) {
if (this->_transects.size() > 0) {
auto &t = this->_transects.front();
QList<CoordInfo_t> list;
......@@ -338,24 +409,54 @@ void CircularSurvey::_rebuildTransectsPhase1(void) {
this->_transects.append(list);
}
this->_needsReversal = false;
} else {
}
// Start calculation.
else {
#ifdef SHOW_CIRCULAR_SURVEY_TIME
auto start = std::chrono::high_resolution_clock::now();
#endif
this->_transects.clear();
// Prepare data.
auto ref = this->_referencePoint;
ref.setAltitude(0);
auto polygon = this->_surveyAreaPolygon.coordinateList();
for (auto &v : polygon) {
v.setAltitude(0);
}
auto safeArea = this->_safeArea;
for (auto &v : safeArea) {
v.setAltitude(0);
}
QGeoCoordinate depot;
snake::BoostPolygon safeAreaENU;
if (this->_depot.isValid() && this->_safeArea.size() >= 3) {
this->_pWorker->update(
this->_depot, this->_safeArea, polygon, this->_referencePoint,
this->_deltaR.rawValue().toDouble() * bu::si::meter,
this->_minLength.rawValue().toDouble() * bu::si::meter,
snake::Angle(this->_deltaAlpha.rawValue().toDouble() *
bu::degree::degree));
depot = this->_depot;
snake::areaToEnu(ref, safeArea, safeAreaENU);
} else {
this->_pWorker->update(
polygon, this->_referencePoint,
this->_deltaR.rawValue().toDouble() * bu::si::meter,
this->_minLength.rawValue().toDouble() * bu::si::meter,
snake::Angle(this->_deltaAlpha.rawValue().toDouble() *
bu::degree::degree));
snake::areaToEnu(ref, polygon, safeAreaENU);
}
auto deltaR =
snake::Length(this->_deltaR.rawValue().toDouble() * bu::si::meter);
auto minLength =
snake::Length(this->_minLength.rawValue().toDouble() * bu::si::meter);
auto deltaAlpha = snake::Angle(this->_deltaAlpha.rawValue().toDouble() *
bu::degree::degree);
auto generator = [ref, depot, polygon, deltaR, deltaAlpha,
minLength](snake::Transects &transects) -> bool {
return circularTransects(ref, depot, polygon, deltaR, deltaAlpha,
minLength, transects);
};
// Start routing worker.
this->_pWorker->route(safeAreaENU, generator);
// Mark transects as dirty.
this->_transectsDirty = true;
#ifdef SHOW_CIRCULAR_SURVEY_TIME
qWarning() << "CS::rebuildTransectsPhase1(): start: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start)
.count()
<< " ms";
#endif
}
}
......@@ -374,8 +475,8 @@ void CircularSurvey::_recalcComplexDistance() {
// no cameraShots in Circular Survey, add if desired
void CircularSurvey::_recalcCameraShots() { _cameraShots = 0; }
void CircularSurvey::_setTransects(CircularSurvey::PtrRoute pRoute) {
this->_pRoute = pRoute;
void CircularSurvey::_setTransects(CircularSurvey::PtrRoutingData pRoute) {
this->_workerOutput = pRoute;
this->_needsStoring = true;
this->_rebuildTransects();
}
......@@ -386,6 +487,237 @@ bool CircularSurvey::calculating() const {
return this->_pWorker->calculating();
}
bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot,
const QList<QGeoCoordinate> &polygon,
snake::Length deltaR, snake::Angle deltaAlpha,
snake::Length minLength, snake::Transects &transects) {
#ifdef SHOW_CIRCULAR_SURVEY_TIME
auto s1 = std::chrono::high_resolution_clock::now();
#endif
// Check preconitions
if (polygon.size() >= 3) {
using namespace boost::units;
// Convert geo polygon to ENU polygon.
snake::BoostPolygon polygonENU;
snake::BoostPoint originENU{0, 0};
snake::BoostPoint depotENU{0, 0};
snake::areaToEnu(ref, polygon, polygonENU);
snake::toENU(ref, ref, originENU);
bool depotValid = depot.isValid();
if (depotValid)
snake::toENU(ref, depot, depotENU);
std::string error;
// Check validity.
if (!bg::is_valid(polygonENU, error)) {
#ifdef DEBUG_CIRCULAR_SURVEY
qWarning() << "CS::circularTransects(): "
"invalid polygon.";
qWarning() << error.c_str();
std::stringstream ss;
ss << bg::wkt(polygonENU);
qWarning() << ss.str().c_str();
#endif
} else {
// Calculate polygon distances and angles.
std::vector<snake::Length> distances;
distances.reserve(polygonENU.outer().size());
std::vector<snake::Angle> angles;
angles.reserve(polygonENU.outer().size());
//#ifdef DEBUG_CIRCULAR_SURVEY
// qWarning() << "CS::circularTransects():";
//#endif
for (const auto &p : polygonENU.outer()) {
snake::Length distance = bg::distance(originENU, p) * si::meter;
distances.push_back(distance);
snake::Angle alpha = (std::atan2(p.get<1>(), p.get<0>())) * si::radian;
alpha = alpha < 0 * si::radian ? alpha + 2 * M_PI * si::radian : alpha;
angles.push_back(alpha);
//#ifdef DEBUG_CIRCULAR_SURVEY
// qWarning() << "distances, angles, coordinates:";
// qWarning() << to_string(distance).c_str();
// qWarning() << to_string(snake::Degree(alpha)).c_str();
// qWarning() << "x = " << p.get<0>() << "y = " << p.get<1>();
//#endif
}
auto rMin = deltaR; // minimal circle radius
snake::Angle alpha1(0 * degree::degree);
snake::Angle alpha2(360 * degree::degree);
// Determine r_min by successive approximation
if (!bg::within(originENU, polygonENU)) {
rMin = bg::distance(originENU, polygonENU) * si::meter;
}
auto rMax = (*std::max_element(distances.begin(),
distances.end())); // maximal circle radius
// Scale parameters and coordinates.
const auto rMinScaled =
ClipperLib::cInt(std::round(rMin.value() * CLIPPER_SCALE));
const auto deltaRScaled =
ClipperLib::cInt(std::round(deltaR.value() * CLIPPER_SCALE));
auto originScaled = ClipperLib::IntPoint{
ClipperLib::cInt(std::round(originENU.get<0>())),
ClipperLib::cInt(std::round(originENU.get<1>()))};
// Generate circle sectors.
auto rScaled = rMinScaled;
const auto nTran = long(std::ceil(((rMax - rMin) / deltaR).value()));
vector<ClipperLib::Path> sectors(nTran, ClipperLib::Path());
const auto nSectors =
long(std::round(((alpha2 - alpha1) / deltaAlpha).value()));
//#ifdef DEBUG_CIRCULAR_SURVEY
// qWarning() << "CS::circularTransects(): sector parameres:";
// qWarning() << "alpha1: " <<
// to_string(snake::Degree(alpha1)).c_str(); qWarning() << "alpha2: "
// << to_string(snake::Degree(alpha2)).c_str(); qWarning() << "n: "
// << to_string((alpha2 - alpha1) / deltaAlpha).c_str(); qWarning()
// << "nSectors: " << nSectors; qWarning() << "rMin: " <<
// to_string(rMin).c_str(); qWarning() << "rMax: " <<
// to_string(rMax).c_str(); qWarning() << "nTran: " << nTran;
//#endif
using ClipperCircle =
GenericCircle<ClipperLib::cInt, ClipperLib::IntPoint>;
for (auto &sector : sectors) {
ClipperCircle circle(rScaled, originScaled);
approximate(circle, nSectors, sector);
rScaled += deltaRScaled;
}
// Clip sectors to polygonENU.
ClipperLib::Path polygonClipper;
snake::BoostPolygon shrinked;
snake::offsetPolygon(polygonENU, shrinked, -0.3);
auto &outer = shrinked.outer();
polygonClipper.reserve(outer.size() - 1);
for (auto it = outer.begin(); it < outer.end() - 1; ++it) {
auto x = ClipperLib::cInt(std::round(it->get<0>() * CLIPPER_SCALE));
auto y = ClipperLib::cInt(std::round(it->get<1>() * CLIPPER_SCALE));
polygonClipper.push_back(ClipperLib::IntPoint{x, y});
}
ClipperLib::Clipper clipper;
clipper.AddPath(polygonClipper, ClipperLib::ptClip, true);
clipper.AddPaths(sectors, ClipperLib::ptSubject, false);
ClipperLib::PolyTree transectsClipper;
clipper.Execute(ClipperLib::ctIntersection, transectsClipper,
ClipperLib::pftNonZero, ClipperLib::pftNonZero);
// Extract transects from PolyTree and convert them to
// BoostLineString
if (depotValid) {
transects.push_back(snake::BoostLineString{depotENU});
}
for (const auto &child : transectsClipper.Childs) {
snake::BoostLineString transect;
transect.reserve(child->Contour.size());
for (const auto &vertex : child->Contour) {
auto x = static_cast<double>(vertex.X) / CLIPPER_SCALE;
auto y = static_cast<double>(vertex.Y) / CLIPPER_SCALE;
transect.push_back(snake::BoostPoint(x, y));
}
transects.push_back(transect);
}
// Join sectors which where slit due to clipping.
const double th = 0.01;
for (auto ito = transects.begin(); ito < transects.end(); ++ito) {
for (auto iti = ito + 1; iti < transects.end(); ++iti) {
auto dist1 = bg::distance(ito->front(), iti->front());
if (dist1 < th) {
snake::BoostLineString temp;
for (auto it = iti->end() - 1; it >= iti->begin(); --it) {
temp.push_back(*it);
}
temp.insert(temp.end(), ito->begin(), ito->end());
*ito = temp;
transects.erase(iti);
break;
}
auto dist2 = bg::distance(ito->front(), iti->back());
if (dist2 < th) {
snake::BoostLineString temp;
temp.insert(temp.end(), iti->begin(), iti->end());
temp.insert(temp.end(), ito->begin(), ito->end());
*ito = temp;
transects.erase(iti);
break;
}
auto dist3 = bg::distance(ito->back(), iti->front());
if (dist3 < th) {
snake::BoostLineString temp;
temp.insert(temp.end(), ito->begin(), ito->end());
temp.insert(temp.end(), iti->begin(), iti->end());
*ito = temp;
transects.erase(iti);
break;
}
auto dist4 = bg::distance(ito->back(), iti->back());
if (dist4 < th) {
snake::BoostLineString temp;
temp.insert(temp.end(), ito->begin(), ito->end());
for (auto it = iti->end() - 1; it >= iti->begin(); --it) {
temp.push_back(*it);
}
*ito = temp;
transects.erase(iti);
break;
}
}
}
// Remove short transects
auto begin = depotValid ? transects.begin() + 1 : transects.begin();
for (auto it = begin; it < transects.end();) {
if (bg::length(*it) < minLength.value()) {
it = transects.erase(it);
} else {
++it;
}
}
if (!depotValid) {
// Move transect with min. distance to the front.
auto minDist = std::numeric_limits<double>::max();
auto minIt = transects.begin();
bool reverse = false;
for (auto it = transects.begin(); it < transects.end(); ++it) {
auto distFront = bg::distance(originENU, it->front());
auto distBack = bg::distance(originENU, it->back());
if (distFront < minDist) {
minDist = distFront;
minIt = it;
reverse = false;
}
if (distBack < minDist) {
minDist = distBack;
minIt = it;
reverse = true;
}
}
// Swap and reverse (if necessary).
if (minIt != transects.begin()) {
auto minTransect = *minIt;
if (reverse) {
snake::BoostLineString rev;
for (auto it = minTransect.end() - 1; it >= minTransect.begin();
--it) {
rev.push_back(*it);
}
minTransect = rev;
}
*minIt = *transects.begin();
*transects.begin() = minTransect;
}
}
#ifdef SHOW_CIRCULAR_SURVEY_TIME
qWarning() << "CS::circularTransects(): transect gen. time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - s1)
.count()
<< " ms";
#endif
return true;
}
}
return false;
}
/*!
\class CircularSurveyComplexItem
\inmodule Wima
......
src/Wima/CircularSurvey.h
View file @ 1ac83998
......@@ -6,13 +6,13 @@
#include "SettingsFact.h"
#include "TransectStyleComplexItem.h"
class CSWorker;
class RoutingWorker;
class RoutingData;
class CircularSurvey : public TransectStyleComplexItem {
Q_OBJECT
public:
using Route = QList<QGeoCoordinate>;
using PtrRoute = QSharedPointer<Route>;
using PtrRoutingData = QSharedPointer<RoutingData>;
/// @param vehicle Vehicle which this is being contructed for
/// @param flyView true: Created for use in the Fly View, false: Created for
......@@ -87,7 +87,7 @@ private slots:
void _rebuildTransectsPhase1(void) final;
void _recalcComplexDistance(void) final;
void _recalcCameraShots(void) final;
void _setTransects(PtrRoute pRoute);
void _setTransects(PtrRoutingData pRoute);
private:
void _appendLoadedMissionItems(QList<MissionItem *> &items,
......@@ -107,9 +107,10 @@ private:
// this value
SettingsFact _minLength;
using PtrWorker = std::shared_ptr<CSWorker>;
using PtrWorker = std::shared_ptr<RoutingWorker>;
PtrWorker _pWorker;
PtrRoute _pRoute;
PtrRoutingData _workerOutput;
QList<QList<QGeoCoordinate>> _rawTransects;
bool _needsStoring;
bool _needsReversal;
bool _hidePolygon;
......
src/Wima/Geometry/WimaCorridor.h
View file @ 1ac83998
#pragma once
#include <QObject>
#include "WimaArea.h"
#include "WimaServiceArea.h"
#include "WimaMeasurementArea.h"
#include <QObject>
class WimaCorridor : public WimaArea
{
Q_OBJECT
class WimaCorridor : public WimaArea {
Q_OBJECT
public:
WimaCorridor(QObject* parent = nullptr);
WimaCorridor(const WimaCorridor& other, QObject* parent = nullptr);
WimaCorridor(QObject *parent = nullptr);
WimaCorridor(const WimaCorridor &other, QObject *parent = nullptr);
WimaCorridor &operator=(const WimaCorridor &other);
WimaCorridor &operator=(const WimaCorridor &other);
// Overrides from WimaPolygon
QString mapVisualQML (void) const { return "WimaCorridorMapVisual.qml";}
QString editorQML (void) const { return "WimaCorridorEditor.qml";}
// Overrides from WimaPolygon
QString mapVisualQML(void) const { return "WimaCorridorMapVisual.qml"; }
QString editorQML(void) const { return "WimaCorridorEditor.qml"; }
// Methodes
void saveToJson (QJsonObject& json);
bool loadFromJson (const QJsonObject& json, QString& errorString);
// Methodes
void saveToJson(QJsonObject &json);
bool loadFromJson(const QJsonObject &json, QString &errorString);
// static Members
static const char* WimaCorridorName;
// static Members
static const char *WimaCorridorName;
// Friends
friend void print(const WimaCorridor& area, QString& outputString);
friend void print(const WimaCorridor& area);
// Friends
friend void print(const WimaCorridor &area, QString &outputString);
friend void print(const WimaCorridor &area);
signals:
public slots:
private:
void init();
void init();
};
src/Wima/Geometry/WimaServiceArea.cc
View file @ 1ac83998
#include "WimaServiceArea.h"
const char *WimaServiceArea::wimaServiceAreaName = "Service Area";
const char *WimaServiceArea::depotLatitudeName = "DepotLatitude";
const char *WimaServiceArea::depotLongitudeName = "DepotLongitude";
const char *WimaServiceArea::depotAltitudeName = "DepotAltitude";
WimaServiceArea::WimaServiceArea(QObject *parent) : WimaArea(parent) { init(); }
WimaServiceArea::WimaServiceArea(const WimaServiceArea &other, QObject *parent)
: WimaArea(other, parent) {
: WimaArea(other, parent), _depot(other.depot()) {
init();
}
......@@ -16,7 +19,7 @@ WimaServiceArea::WimaServiceArea(const WimaServiceArea &other, QObject *parent)
*/
WimaServiceArea &WimaServiceArea::operator=(const WimaServiceArea &other) {
WimaArea::operator=(other);
this->setDepot(other.depot());
return *this;
}
......@@ -34,18 +37,41 @@ bool WimaServiceArea::setDepot(const QGeoCoordinate &coordinate) {
void WimaServiceArea::saveToJson(QJsonObject &json) {
this->WimaArea::saveToJson(json);
json[areaTypeName] = wimaServiceAreaName;
json[depotLatitudeName] = _depot.latitude();
json[depotLongitudeName] = _depot.longitude();
json[depotAltitudeName] = _depot.altitude();
}
bool WimaServiceArea::loadFromJson(const QJsonObject &json,
QString &errorString) {
bool retVal = false;
if (this->WimaArea::loadFromJson(json, errorString)) {
bool retVal = true;
// code for loading here
return retVal;
} else {
qWarning() << errorString;
return false;
double lat = 0;
if (json.contains(depotLatitudeName) &&
json[depotLatitudeName].isDouble()) {
lat = json[depotLatitudeName].toDouble();
double lon = 0;
if (json.contains(depotLongitudeName) &&
json[depotLongitudeName].isDouble()) {
lon = json[depotLongitudeName].toDouble();
double alt = 0;
if (json.contains(depotAltitudeName) &&
json[depotAltitudeName].isDouble()) {
alt = json[depotAltitudeName].toDouble();
this->setDepot(QGeoCoordinate(lat, lon, alt));
retVal = true;
} else {
errorString = "Not able to load depot altitude.";
}
} else {
errorString = "Not able to load depot longitude.";
}
} else {
errorString = "Not able to load depot latitude.";
}
retVal = true;
}
return retVal;
}
void print(const WimaServiceArea &area) {
......
src/Wima/Geometry/WimaServiceArea.h
View file @ 1ac83998
......@@ -30,6 +30,9 @@ public:
// static Members
static const char *wimaServiceAreaName;
static const char *depotLatitudeName;
static const char *depotLongitudeName;
static const char *depotAltitudeName;
signals:
void depotChanged(void);
......
src/Wima/WaypointManager/DefaultManager.cpp
View file @ 1ac83998
......@@ -5,274 +5,273 @@
#include "MissionSettingsItem.h"
#include "SimpleMissionItem.h"
WaypointManager::DefaultManager::DefaultManager(Settings &settings,
AreaInterface &interface)
: ManagerBase(settings)
, _areaInterface(&interface)
{
}
void WaypointManager::DefaultManager::clear()
{
_dirty = true;
_waypoints.clear();
_currentWaypoints.clear();
_missionItems.clearAndDeleteContents();
_currentMissionItems.clearAndDeleteContents();
_waypointsVariant.clear();
_currentWaypointsVariant.clear();
: ManagerBase(settings), _areaInterface(&interface) {}
void WaypointManager::DefaultManager::clear() {
_dirty = true;
_waypoints.clear();
_currentWaypoints.clear();
_missionItems.clearAndDeleteContents();
_currentMissionItems.clearAndDeleteContents();
_waypointsVariant.clear();
_currentWaypointsVariant.clear();
}
bool WaypointManager::DefaultManager::update()
{
// extract waypoints
_currentWaypoints.clear();
Slicer::update(_waypoints, _currentWaypoints);
return _worker();
bool WaypointManager::DefaultManager::update() {
// extract waypoints
_currentWaypoints.clear();
Slicer::update(_waypoints, _currentWaypoints);
return _worker();
}
bool WaypointManager::DefaultManager::next()
{
// extract waypoints
_currentWaypoints.clear();
Slicer::next(_waypoints, _currentWaypoints);
return _worker();
bool WaypointManager::DefaultManager::next() {
// extract waypoints
_currentWaypoints.clear();
Slicer::next(_waypoints, _currentWaypoints);
return _worker();
}
bool WaypointManager::DefaultManager::previous()
{
// extract waypoints
_currentWaypoints.clear();
Slicer::previous(_waypoints, _currentWaypoints);
return _worker();
bool WaypointManager::DefaultManager::previous() {
// extract waypoints
_currentWaypoints.clear();
Slicer::previous(_waypoints, _currentWaypoints);
return _worker();
}
bool WaypointManager::DefaultManager::reset()
{
// extract waypoints
_currentWaypoints.clear();
Slicer::reset(_waypoints, _currentWaypoints);
return _worker();
bool WaypointManager::DefaultManager::reset() {
// extract waypoints
_currentWaypoints.clear();
Slicer::reset(_waypoints, _currentWaypoints);
return _worker();
}
bool WaypointManager::DefaultManager::_insertMissionItem(const QGeoCoordinate &c,
size_t index,
QmlObjectListModel &list,
bool doUpdate)
{
using namespace WaypointManager::Utils;
if ( !insertMissionItem(c,
index /*insertion index*/,
list,
_settings->vehicle(),
_settings->isFlyView(),
&list /*parent*/,
doUpdate /*do update*/) )
{
qWarning("WaypointManager::DefaultManager::next(): insertMissionItem failed.");
Q_ASSERT(false);
return false;
}
return true;
bool WaypointManager::DefaultManager::_insertMissionItem(
const QGeoCoordinate &c, size_t index, QmlObjectListModel &list,
bool doUpdate) {
using namespace WaypointManager::Utils;
if (!insertMissionItem(c, index /*insertion index*/, list,
_settings->vehicle(), _settings->isFlyView(),
&list /*parent*/, doUpdate /*do update*/)) {
qWarning(
"WaypointManager::DefaultManager::next(): insertMissionItem failed.");
Q_ASSERT(false);
return false;
}
return true;
}
bool WaypointManager::DefaultManager::_insertMissionItem(const QGeoCoordinate &c,
size_t index,
bool doUpdate)
{
return _insertMissionItem(c, index, _currentMissionItems, doUpdate);
bool WaypointManager::DefaultManager::_insertMissionItem(
const QGeoCoordinate &c, size_t index, bool doUpdate) {
return _insertMissionItem(c, index, _currentMissionItems, doUpdate);
}
bool WaypointManager::DefaultManager::_calcShortestPath(
const QGeoCoordinate &start,
const QGeoCoordinate &destination,
QVector<QGeoCoordinate> &path)
{
using namespace GeoUtilities;
using namespace PolygonCalculus;
QPolygonF joinedArea2D;
toCartesianList(_areaInterface->joinedArea()->coordinateList(), /*origin*/ start, joinedArea2D);
QPointF start2D(0,0);
QPointF end2D;
toCartesian(destination, start, end2D);
QVector<QPointF> path2DOut;
bool retVal = PolygonCalculus::shortestPath(joinedArea2D, start2D, end2D, path2DOut);
toGeoList(path2DOut, /*origin*/ start, path);
return retVal;
const QGeoCoordinate &start, const QGeoCoordinate &destination,
QVector<QGeoCoordinate> &path) {
using namespace GeoUtilities;
using namespace PolygonCalculus;
QPolygonF joinedArea2D;
toCartesianList(_areaInterface->joinedArea()->coordinateList(),
/*origin*/ start, joinedArea2D);
QPointF start2D(0, 0);
QPointF end2D;
toCartesian(destination, start, end2D);
QVector<QPointF> path2DOut;
bool retVal =
PolygonCalculus::shortestPath(joinedArea2D, start2D, end2D, path2DOut);
toGeoList(path2DOut, /*origin*/ start, path);
return retVal;
}
bool WaypointManager::DefaultManager::_worker()
{
// Precondition:
// _waypoints must contain valid coordinates.
// Slicer must be called befor invoking this function.
// E.g. Slicer::reset(_waypoints, _currentWaypoints);
bool WaypointManager::DefaultManager::_worker() {
// Precondition:
// _waypoints must contain valid coordinates.
// Slicer must be called befor invoking this function.
// E.g. Slicer::reset(_waypoints, _currentWaypoints);
using namespace WaypointManager::Utils;
using namespace WaypointManager::Utils;
if (_waypoints.count() < 1 || !_settings->valid()) {
return false;
}
if (_dirty) {
_missionItems.clearAndDeleteContents();
_waypointsVariant.clear();
// No initialization of _missionItems, don't need MissionSettingsItem here.
for (size_t i = 0; i < size_t(_waypoints.size()); ++i) {
auto &c = _waypoints.at(i);
_insertMissionItem(c, _missionItems.count(), _missionItems, false /*update list*/);
_waypointsVariant.push_back(QVariant::fromValue(c));
}
updateHirarchy(_missionItems);
updateSequenceNumbers(_missionItems, 1); // Start with 1, since MissionSettingsItem missing.
_dirty = false;
}
if (_waypoints.count() < 1 || !_settings->valid()) {
return false;
}
_currentMissionItems.clearAndDeleteContents();
initialize(_currentMissionItems, _settings->vehicle(), _settings->isFlyView());
// Calculate path from home to first waypoint.
QVector<QGeoCoordinate> arrivalPath;
if ( !_calcShortestPath(_settings->homePosition(), _currentWaypoints.first(), arrivalPath) ) {
qWarning("WaypointManager::DefaultManager::next(): Not able to calc path from home position to first waypoint.");
return false;
}
if (!arrivalPath.empty())
arrivalPath.removeFirst();
if (!arrivalPath.empty())
arrivalPath.removeLast();
// calculate path from last waypoint to home
QVector<QGeoCoordinate> returnPath;
if ( !_calcShortestPath(_currentWaypoints.last(), _settings->homePosition(), returnPath) ) {
qWarning("WaypointManager::DefaultManager::next(): Not able to calc path from home position to last waypoint.");
return false;
}
if (!returnPath.empty())
returnPath.removeFirst();
if (!returnPath.empty())
returnPath.removeLast();
// Create mission items.
// Set home position of MissionSettingsItem.
MissionSettingsItem* settingsItem = _currentMissionItems.value<MissionSettingsItem *>(0);
if (settingsItem == nullptr) {
Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false;
}
settingsItem->setCoordinate(_settings->homePosition());
// First mission item is takeoff command.
_insertMissionItem(_settings->homePosition(), 1 /*insertion index*/, false /*do update*/);
SimpleMissionItem *takeoffItem = _currentMissionItems.value<SimpleMissionItem*>(1);
if (takeoffItem == nullptr) {
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
Q_ASSERT(takeoffItem != nullptr);
return false;
}
makeTakeOffCmd(takeoffItem, _settings->masterController()->managerVehicle());
// Create change speed item.
_insertMissionItem(_settings->homePosition(), 2 /*insertion index*/, false /*do update*/);
SimpleMissionItem *speedItem = _currentMissionItems.value<SimpleMissionItem*>(2);
if (speedItem == nullptr) {
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
Q_ASSERT(speedItem != nullptr);
return false;
}
makeSpeedCmd(speedItem, _settings->arrivalReturnSpeed());
// insert arrival path
for (auto coordinate : arrivalPath) {
_insertMissionItem(coordinate,
_currentMissionItems.count() /*insertion index*/,
false /*do update*/);
}
// Create change speed item (after arrival path).
int index = _currentMissionItems.count();
_insertMissionItem(_currentWaypoints.first(), index /*insertion index*/, false /*do update*/);
speedItem = _currentMissionItems.value<SimpleMissionItem*>(index);
if (speedItem == nullptr) {
Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false;
}
makeSpeedCmd(speedItem, _settings->flightSpeed());
// Insert slice.
for (auto coordinate : _currentWaypoints) {
_insertMissionItem(coordinate,
_currentMissionItems.count() /*insertion index*/,
false /*do update*/);
}
// Create change speed item, after slice.
index = _currentMissionItems.count();
_insertMissionItem(_currentWaypoints.last(), index /*insertion index*/, false /*do update*/);
speedItem = _currentMissionItems.value<SimpleMissionItem*>(index);
if (speedItem == nullptr) {
Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false;
}
makeSpeedCmd(speedItem, _settings->arrivalReturnSpeed());
// Insert return path coordinates.
for (auto coordinate : returnPath) {
_insertMissionItem(coordinate,
_currentMissionItems.count() /*insertion index*/,
false /*do update*/);
}
// Set land command for last mission item.
index = _currentMissionItems.count();
_insertMissionItem(_settings->homePosition(), index /*insertion index*/, false /*do update*/);
SimpleMissionItem *landItem = _currentMissionItems.value<SimpleMissionItem*>(index);
if (landItem == nullptr) {
Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false;
if (_dirty) {
_missionItems.clearAndDeleteContents();
_waypointsVariant.clear();
// No initialization of _missionItems, don't need MissionSettingsItem here.
for (size_t i = 0; i < size_t(_waypoints.size()); ++i) {
auto &c = _waypoints.at(i);
_insertMissionItem(c, _missionItems.count(), _missionItems,
false /*update list*/);
_waypointsVariant.push_back(QVariant::fromValue(c));
}
makeLandCmd(landItem, _settings->masterController()->managerVehicle());
// Set altitude.
for (int i = 1; i < _currentMissionItems.count(); ++i) {
SimpleMissionItem *item = _currentMissionItems.value<SimpleMissionItem *>(i);
if (item == nullptr) {
Q_ASSERT(false);
qWarning("WimaController::updateAltitude(): nullptr");
return false;
}
item->altitude()->setRawValue(_settings->altitude());
updateHirarchy(_missionItems);
updateSequenceNumbers(
_missionItems, 1); // Start with 1, since MissionSettingsItem missing.
_dirty = false;
}
_currentMissionItems.clearAndDeleteContents();
initialize(_currentMissionItems, _settings->vehicle(),
_settings->isFlyView());
// Calculate path from home to first waypoint.
QVector<QGeoCoordinate> arrivalPath;
if (!_calcShortestPath(_settings->homePosition(), _currentWaypoints.first(),
arrivalPath)) {
qWarning("WaypointManager::DefaultManager::next(): Not able to calc path "
"from home position to first waypoint.");
qWarning() << "from: " << _settings->homePosition();
qWarning() << "to: " << _currentWaypoints.first();
return false;
}
if (!arrivalPath.empty())
arrivalPath.removeFirst();
if (!arrivalPath.empty())
arrivalPath.removeLast();
// calculate path from last waypoint to home
QVector<QGeoCoordinate> returnPath;
if (!_calcShortestPath(_currentWaypoints.last(), _settings->homePosition(),
returnPath)) {
qWarning("WaypointManager::DefaultManager::next(): Not able to calc path "
"from home position to last waypoint.");
qWarning() << "from: " << _currentWaypoints.last();
qWarning() << "to: " << _settings->homePosition();
return false;
}
if (!returnPath.empty())
returnPath.removeFirst();
if (!returnPath.empty())
returnPath.removeLast();
// Create mission items.
// Set home position of MissionSettingsItem.
MissionSettingsItem *settingsItem =
_currentMissionItems.value<MissionSettingsItem *>(0);
if (settingsItem == nullptr) {
Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false;
}
settingsItem->setCoordinate(_settings->homePosition());
// First mission item is takeoff command.
_insertMissionItem(_settings->homePosition(), 1 /*insertion index*/,
false /*do update*/);
SimpleMissionItem *takeoffItem =
_currentMissionItems.value<SimpleMissionItem *>(1);
if (takeoffItem == nullptr) {
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
Q_ASSERT(takeoffItem != nullptr);
return false;
}
makeTakeOffCmd(takeoffItem, _settings->masterController()->managerVehicle());
// Create change speed item.
_insertMissionItem(_settings->homePosition(), 2 /*insertion index*/,
false /*do update*/);
SimpleMissionItem *speedItem =
_currentMissionItems.value<SimpleMissionItem *>(2);
if (speedItem == nullptr) {
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
Q_ASSERT(speedItem != nullptr);
return false;
}
makeSpeedCmd(speedItem, _settings->arrivalReturnSpeed());
// insert arrival path
for (auto coordinate : arrivalPath) {
_insertMissionItem(coordinate,
_currentMissionItems.count() /*insertion index*/,
false /*do update*/);
}
// Create change speed item (after arrival path).
int index = _currentMissionItems.count();
_insertMissionItem(_currentWaypoints.first(), index /*insertion index*/,
false /*do update*/);
speedItem = _currentMissionItems.value<SimpleMissionItem *>(index);
if (speedItem == nullptr) {
Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false;
}
makeSpeedCmd(speedItem, _settings->flightSpeed());
// Insert slice.
for (auto coordinate : _currentWaypoints) {
_insertMissionItem(coordinate,
_currentMissionItems.count() /*insertion index*/,
false /*do update*/);
}
// Create change speed item, after slice.
index = _currentMissionItems.count();
_insertMissionItem(_currentWaypoints.last(), index /*insertion index*/,
false /*do update*/);
speedItem = _currentMissionItems.value<SimpleMissionItem *>(index);
if (speedItem == nullptr) {
Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false;
}
makeSpeedCmd(speedItem, _settings->arrivalReturnSpeed());
// Insert return path coordinates.
for (auto coordinate : returnPath) {
_insertMissionItem(coordinate,
_currentMissionItems.count() /*insertion index*/,
false /*do update*/);
}
// Set land command for last mission item.
index = _currentMissionItems.count();
_insertMissionItem(_settings->homePosition(), index /*insertion index*/,
false /*do update*/);
SimpleMissionItem *landItem =
_currentMissionItems.value<SimpleMissionItem *>(index);
if (landItem == nullptr) {
Q_ASSERT(false);
qWarning("WaypointManager::DefaultManager::next(): nullptr.");
return false;
}
makeLandCmd(landItem, _settings->masterController()->managerVehicle());
// Set altitude.
for (int i = 1; i < _currentMissionItems.count(); ++i) {
SimpleMissionItem *item =
_currentMissionItems.value<SimpleMissionItem *>(i);
if (item == nullptr) {
Q_ASSERT(false);
qWarning("WimaController::updateAltitude(): nullptr");
return false;
}
// Update list _currentMissionItems.
updateHirarchy(_currentMissionItems);
updateSequenceNumbers(_currentMissionItems);
// Prepend arrival path to slice.
for ( long i = arrivalPath.size()-1; i >=0; --i )
_currentWaypoints.push_front(arrivalPath[i]);
_currentWaypoints.push_front(_settings->homePosition());
// Append return path to slice.
for ( auto c : returnPath )
_currentWaypoints.push_back(c);
_currentWaypoints.push_back(_settings->homePosition());
// Create variant list.
_currentWaypointsVariant.clear();
for ( auto c : _currentWaypoints)
_currentWaypointsVariant.push_back(QVariant::fromValue(c));
return true;
item->altitude()->setRawValue(_settings->altitude());
}
// Update list _currentMissionItems.
updateHirarchy(_currentMissionItems);
updateSequenceNumbers(_currentMissionItems);
// Prepend arrival path to slice.
for (long i = arrivalPath.size() - 1; i >= 0; --i)
_currentWaypoints.push_front(arrivalPath[i]);
_currentWaypoints.push_front(_settings->homePosition());
// Append return path to slice.
for (auto c : returnPath)
_currentWaypoints.push_back(c);
_currentWaypoints.push_back(_settings->homePosition());
// Create variant list.
_currentWaypointsVariant.clear();
for (auto c : _currentWaypoints)
_currentWaypointsVariant.push_back(QVariant::fromValue(c));
return true;
}
src/Wima/WimaController.cc
View file @ 1ac83998
......@@ -424,21 +424,24 @@ bool WimaController::setWimaPlanData(QSharedPointer<WimaPlanData> planData) {
emit visualItemsChanged();
// extract mission items
QList<MissionItem> tempMissionItems = planData->missionItems();
auto tempMissionItems = planData->missionItems();
if (tempMissionItems.size() < 1) {
qWarning("WimaController: Mission items from WimaPlaner empty!");
return false;
}
for (auto item : tempMissionItems) {
_defaultWM.push_back(item.coordinate());
qWarning() << "WimaController:";
for (auto *item : tempMissionItems) {
qWarning() << item->coordinate();
_defaultWM.push_back(item->coordinate());
}
_WMSettings.setHomePosition(QGeoCoordinate(
_serviceArea.depot().latitude(), _serviceArea.depot().longitude(), 0));
qWarning() << "service area depot: " << _serviceArea.depot();
if (!_defaultWM.reset()) {
Q_ASSERT(false);
qWarning() << "_defaultWM.reset() failed";
return false;
}
......
src/Wima/WimaPlanData.cc
View file @ 1ac83998
......@@ -110,7 +110,7 @@ const QList<const WimaAreaData *> &WimaPlanData::areaList() const {
return _areaList;
}
const QList<MissionItem> &WimaPlanData::missionItems() const {
const QList<MissionItem *> &WimaPlanData::missionItems() const {
return _missionItems;
}
......
src/Wima/WimaPlanData.h
View file @ 1ac83998
......@@ -29,7 +29,7 @@ public:
void clear();
const QList<const WimaAreaData *> &areaList() const;
const QList<MissionItem> &missionItems() const;
const QList<MissionItem *> &missionItems() const;
signals:
void areaListChanged();
......@@ -44,5 +44,5 @@ private:
WimaMeasurementAreaData _measurementArea;
QList<const WimaAreaData *> _areaList;
QList<MissionItem> _missionItems;
QList<MissionItem *> _missionItems;
};
src/Wima/WimaPlaner.cc
View file @ 1ac83998
......@@ -740,6 +740,10 @@ QSharedPointer<WimaPlanData> WimaPlaner::toPlanData() {
QList<MissionItem *> missionItems;
_TSComplexItem->appendMissionItems(missionItems, nullptr);
// store mavlink commands
qWarning() << "WimaPlaner";
for (auto *item : missionItems) {
qWarning() << item->coordinate();
}
planData->append(missionItems);
return planData;
}
......
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