diff --git a/Paths/KlingenbachTest.wima b/Paths/KlingenbachTest.wima index bbc408395a9cd66cc9b8c591dc95115606a2fd09..98a1e5e7e5ee0791b8473de2d386e332562187b4 100644 --- a/Paths/KlingenbachTest.wima +++ b/Paths/KlingenbachTest.wima @@ -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, diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml index 98a1e3d9a03787e796db55993f3a372c5498244c..fcb3e48b718c638568742a13e74aa6b4e06bc0b1 100644 --- a/src/FlightDisplay/FlightDisplayViewMap.qml +++ b/src/FlightDisplay/FlightDisplayViewMap.qml @@ -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" diff --git a/src/Wima/CSWorker.cpp b/src/Wima/CSWorker.cpp index c0f416b0dba597898c04e136fb95fecbbcc3b88c..aa40ec0059c6931e7365995f20b4db2e925eac1f 100644 --- a/src/Wima/CSWorker.cpp +++ b/src/Wima/CSWorker.cpp @@ -1,24 +1,18 @@ #include "CSWorker.h" -// Wima -#define CLIPPER_SCALE 10000 -#include "clipper/clipper.hpp" -template ClipperLib::cInt get(ClipperLib::IntPoint &p); -#include "Geometry/GenericCircle.h" // std #include // Qt #include -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"); }); +} -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 &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 &safeArea, - const QList &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 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 distances; - distances.reserve(polygonENU.outer().size()); - std::vector 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 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; - for (auto §or : 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(vertex.X) / CLIPPER_SCALE; - auto y = static_cast(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::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::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 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::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::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."; } diff --git a/src/Wima/CSWorker.h b/src/Wima/CSWorker.h index d9c1a01f59fc42d3098c9606d5bcc6b01d1d6132..9df3ce64e64fe40a63de333a7bce959cd2d7a53a 100644 --- a/src/Wima/CSWorker.h +++ b/src/Wima/CSWorker.h @@ -1,44 +1,43 @@ #pragma once #include -#include #include #include #include "snake.h" #include #include +#include #include +struct RoutingData { + snake::BoostLineString route; + snake::Transects transects; + std::vector 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; public: - using Route = QList; - using PtrRoute = QSharedPointer; + using PtrRoutingData = QSharedPointer; + using Generator = std::function; - CSWorker(QObject *parent = nullptr); - ~CSWorker() override; + RoutingWorker(QObject *parent = nullptr); + ~RoutingWorker() override; bool calculating(); public slots: - void update(const QList &polygon, - const QGeoCoordinate &origin, snake::Length deltaR, - snake::Length minLength, snake::Angle deltaAlpha); - void update(const QGeoCoordinate &depot, - const QList &safeArea, - const QList &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 _safeArea; - QList _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; diff --git a/src/Wima/CircularSurvey.cc b/src/Wima/CircularSurvey.cc index c07b1607577daf3ab55627028de50dd8e17a87af..7e4c4bef620e7be64f0dab4bc164df4885241c78 100644 --- a/src/Wima/CircularSurvey.cc +++ b/src/Wima/CircularSurvey.cc @@ -5,6 +5,13 @@ #include "QGCApplication.h" // Wima #include "snake.h" +#define CLIPPER_SCALE 10000 +#include "clipper/clipper.hpp" + +template 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 #include @@ -18,6 +25,11 @@ private: Functor fun; }; +bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot, + const QList &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()), _needsStoring(false), - _needsReversal(false) { + _pWorker(std::make_unique()), _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"); - 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 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 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::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 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::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 &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 distances; + distances.reserve(polygonENU.outer().size()); + std::vector 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 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; + for (auto §or : 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(vertex.X) / CLIPPER_SCALE; + auto y = static_cast(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::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::high_resolution_clock::now() - s1) + .count() + << " ms"; +#endif + return true; + } + } + return false; +} /*! \class CircularSurveyComplexItem \inmodule Wima diff --git a/src/Wima/CircularSurvey.h b/src/Wima/CircularSurvey.h index 8f5c5b41fa9bfe1481cc3fc675e935f0cf2b5761..4305536c78bd68603442a01d2935741594d22c99 100644 --- a/src/Wima/CircularSurvey.h +++ b/src/Wima/CircularSurvey.h @@ -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; - using PtrRoute = QSharedPointer; + using PtrRoutingData = QSharedPointer; /// @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 &items, @@ -107,9 +107,10 @@ private: // this value SettingsFact _minLength; - using PtrWorker = std::shared_ptr; + using PtrWorker = std::shared_ptr; PtrWorker _pWorker; - PtrRoute _pRoute; + PtrRoutingData _workerOutput; + QList> _rawTransects; bool _needsStoring; bool _needsReversal; bool _hidePolygon; diff --git a/src/Wima/Geometry/WimaCorridor.h b/src/Wima/Geometry/WimaCorridor.h index b0019d5bf62f5da4bed61e89df2f8f9bdf8752de..aecaa66badb0e415b9bae63d1128cebfeca3a2cc 100644 --- a/src/Wima/Geometry/WimaCorridor.h +++ b/src/Wima/Geometry/WimaCorridor.h @@ -1,41 +1,35 @@ #pragma once -#include #include "WimaArea.h" -#include "WimaServiceArea.h" -#include "WimaMeasurementArea.h" +#include -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(); }; - - diff --git a/src/Wima/Geometry/WimaServiceArea.cc b/src/Wima/Geometry/WimaServiceArea.cc index 81ea30023cf8515e4499219a3e1010c19ec8af83..0a482e0c1e9de30c2d99fce05959b6d13e54590f 100644 --- a/src/Wima/Geometry/WimaServiceArea.cc +++ b/src/Wima/Geometry/WimaServiceArea.cc @@ -1,11 +1,14 @@ #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) { diff --git a/src/Wima/Geometry/WimaServiceArea.h b/src/Wima/Geometry/WimaServiceArea.h index 90358361f622a3aab450c82a00a9dcc4d0bf1b6a..283aa9fa58c51b61eeaf99995dd276253be4a390 100644 --- a/src/Wima/Geometry/WimaServiceArea.h +++ b/src/Wima/Geometry/WimaServiceArea.h @@ -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); diff --git a/src/Wima/WaypointManager/DefaultManager.cpp b/src/Wima/WaypointManager/DefaultManager.cpp index a7812c4af462398c7459c0203ff019d29a3b507b..97fd1e7c93e8398acb00d39ac94977dde30339f8 100644 --- a/src/Wima/WaypointManager/DefaultManager.cpp +++ b/src/Wima/WaypointManager/DefaultManager.cpp @@ -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 &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 path2DOut; - - bool retVal = PolygonCalculus::shortestPath(joinedArea2D, start2D, end2D, path2DOut); - toGeoList(path2DOut, /*origin*/ start, path); - - return retVal; + const QGeoCoordinate &start, const QGeoCoordinate &destination, + QVector &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 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 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 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(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(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(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(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(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(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(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 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 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(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(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(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(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(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(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(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; } diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index 40fb142af27a85dc4892f88c5b1c412a3953a8c3..f8441f52a78c6098a2343a97975d04a0d3e85e0b 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -424,21 +424,24 @@ bool WimaController::setWimaPlanData(QSharedPointer planData) { emit visualItemsChanged(); // extract mission items - QList 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; } diff --git a/src/Wima/WimaPlanData.cc b/src/Wima/WimaPlanData.cc index 394c53c25ce55c9f2b477b91487cec47854261e7..e1ea35911b9b77c657d368f7d441191125500fcd 100644 --- a/src/Wima/WimaPlanData.cc +++ b/src/Wima/WimaPlanData.cc @@ -110,7 +110,7 @@ const QList &WimaPlanData::areaList() const { return _areaList; } -const QList &WimaPlanData::missionItems() const { +const QList &WimaPlanData::missionItems() const { return _missionItems; } diff --git a/src/Wima/WimaPlanData.h b/src/Wima/WimaPlanData.h index 221267e03427306c77883c5a9861e2af4e8a9a32..e073af0030fa0c9c64209d6136fe565e13be251b 100644 --- a/src/Wima/WimaPlanData.h +++ b/src/Wima/WimaPlanData.h @@ -29,7 +29,7 @@ public: void clear(); const QList &areaList() const; - const QList &missionItems() const; + const QList &missionItems() const; signals: void areaListChanged(); @@ -44,5 +44,5 @@ private: WimaMeasurementAreaData _measurementArea; QList _areaList; - QList _missionItems; + QList _missionItems; }; diff --git a/src/Wima/WimaPlaner.cc b/src/Wima/WimaPlaner.cc index e0a5cbe789ee9c74baf265d2575ee5885d03ee1c..192b7a42b8d38ded72ffb441aee29b948848a77c 100644 --- a/src/Wima/WimaPlaner.cc +++ b/src/Wima/WimaPlaner.cc @@ -740,6 +740,10 @@ QSharedPointer WimaPlaner::toPlanData() { QList missionItems; _TSComplexItem->appendMissionItems(missionItems, nullptr); // store mavlink commands + qWarning() << "WimaPlaner"; + for (auto *item : missionItems) { + qWarning() << item->coordinate(); + } planData->append(missionItems); return planData; }