From 99b270287c4fdecbc777ed51534f6989390dd1f6 Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Mon, 29 Jun 2020 17:28:20 +0200 Subject: [PATCH] SnakeWorker issues solved, about to add tile visualization. --- src/Snake/snake.cpp | 103 +++++++++++++++---- src/Snake/snake.h | 16 ++- src/Wima/WimaAreaData.h | 6 +- src/Wima/WimaController.cc | 168 ++++++++++--------------------- src/Wima/WimaController.h | 61 +++++------ src/Wima/WimaControllerDetail.cc | 32 +++--- src/Wima/WimaControllerDetail.h | 5 +- src/Wima/WimaPlaner.cc | 23 +++-- src/comm/utilities.h | 4 +- src/main.cc | 6 +- 10 files changed, 231 insertions(+), 193 deletions(-) diff --git a/src/Snake/snake.cpp b/src/Snake/snake.cpp index 3b5197d75..015166814 100644 --- a/src/Snake/snake.cpp +++ b/src/Snake/snake.cpp @@ -4,6 +4,10 @@ #include "clipper/clipper.hpp" #define CLIPPER_SCALE 10000 +#ifndef NDEBUG +//#define SHOW_TIME +#endif + using namespace snake_geometry; using namespace std; @@ -71,6 +75,7 @@ bool Scenario::_areas2enu() } bg::correct(_serviceAreaENU); polygonCenter(_serviceAreaENU, _homePositionENU); + fromENU(_geoOrigin, Point3D{_homePositionENU.get<0>(), _homePositionENU.get<1>(), 0}, _homePosition); _corridorENU.clear(); if (_corridor.geoPolygon.size() > 0){ @@ -146,6 +151,8 @@ bool Scenario::_calculateTiles(double tileWidth, double tileHeight, double minTi { _tilesENU.clear(); _tileCenterPointsENU.clear(); + _tiles.clear(); + _tileCenterPoints.clear(); if (tileWidth <= 0 || tileHeight <= 0 || minTileArea <= 0) { errorString = "Parameters tileWidth, tileHeight, minTileArea must be positive."; @@ -223,6 +230,23 @@ bool Scenario::_calculateTiles(double tileWidth, double tileHeight, double minTi return false; } + for ( auto tile : _tilesENU){ + GeoPoint3DList geoTile; + for ( int i=0; i < int(tile.outer().size())-1; ++i) { + BoostPoint vertex(tile.outer()[i]); + GeoPoint3D geoVertex; + fromENU(_geoOrigin, Point3D{vertex.get<0>(), vertex.get<1>(), 0}, geoVertex); + geoTile.push_back(geoVertex); + } + _tiles.push_back(geoTile); + } + + for ( auto vertex : _tileCenterPointsENU){ + GeoPoint3D geoVertex; + fromENU(_geoOrigin, Point3D{vertex.get<0>(), vertex.get<1>(), 0}, geoVertex); + _tileCenterPoints.push_back(geoVertex); + } + return true; } @@ -240,8 +264,9 @@ bool Scenario::_calculateJoinedArea() // Corridor overlaping with measurement area? if ( bg::intersects(_corridorENU, _measurementAreaENU) ) { // Corridor overlaping with service area? - if ( bg::intersects(_corridorENU, _serviceAreaENU) ) + if ( bg::intersects(_corridorENU, _serviceAreaENU) ) { corridor_is_connection = true; + } } } @@ -291,12 +316,16 @@ bool FlightPlan::generate(double lineDistance, double minTransectLength) #ifndef NDEBUG _PathVertices.clear(); #endif +#ifdef SHOW_TIME auto start = std::chrono::high_resolution_clock::now(); +#endif if (!_generateTransects(lineDistance, minTransectLength)) return false; +#ifdef SHOW_TIME auto delta = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start); cout << endl; cout << "Execution time _generateTransects(): " << delta.count() << " ms" << endl; +#endif //======================================= // Route Transects using Google or-tools. @@ -330,10 +359,14 @@ bool FlightPlan::generate(double lineDistance, double minTransectLength) RoutingDataModel_t dataModel; Matrix connectionGraph(n1, n1); +#ifdef SHOW_TIME start = std::chrono::high_resolution_clock::now(); +#endif _generateRoutingModel(vertices, jAreaOffset, n0, dataModel, connectionGraph); +#ifdef SHOW_TIME delta = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start); cout << "Execution time _generateRoutingModel(): " << delta.count() << " ms" << endl; +#endif // Create Routing Index Manager. RoutingIndexManager manager(dataModel.distanceMatrix.getN(), dataModel.numVehicles, @@ -350,6 +383,9 @@ bool FlightPlan::generate(double lineDistance, double minTransectLength) return dataModel.distanceMatrix.get(from_node, to_node); }); + // Define cost of each arc. + routing.SetArcCostEvaluatorOfAllVehicles(transit_callback_index); + // Define Constraints. size_t n = _transects.size()*2; @@ -373,9 +409,6 @@ bool FlightPlan::generate(double lineDistance, double minTransectLength) solver->AddConstraint(c); } - // Define cost of each arc. - routing.SetArcCostEvaluatorOfAllVehicles(transit_callback_index); - // Setting first solution heuristic. RoutingSearchParameters searchParameters = DefaultRoutingSearchParameters(); @@ -386,10 +419,14 @@ bool FlightPlan::generate(double lineDistance, double minTransectLength) searchParameters.set_allocated_time_limit(tMax); // Solve the problem. +#ifdef SHOW_TIME start = std::chrono::high_resolution_clock::now(); +#endif const Assignment* solution = routing.SolveWithParameters(searchParameters); +#ifdef SHOW_TIME delta = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start); cout << "Execution time routing.SolveWithParameters(): " << delta.count() << " ms" << endl; +#endif if (!solution || solution->Size() <= 1){ errorString = "Not able to solve the routing problem."; @@ -413,14 +450,25 @@ bool FlightPlan::generate(double lineDistance, double minTransectLength) { _waypointsENU.push_back(vertices[route[0]]); vector pathIdx; + long arrivalPathLength = 0; for (long i=0; i 0; --i) + _returnPathIdx.push_back(_waypointsENU.size()-i); + + for (long i=0; i < arrivalPathLength; ++i) + _arrivalPathIdx.push_back(i); } // Back transform waypoints. @@ -431,18 +479,6 @@ bool FlightPlan::generate(double lineDistance, double minTransectLength) _waypoints.push_back(GeoPoint2D{geoVertex[0], geoVertex[1]}); } - // Reconstruct arrival path; - int i = 0; - for (; route[i] >= n_t; ++i) - _arrivalPathIdx.push_back(i); - _arrivalPathIdx.push_back(i); // vertex of first transect - - // Reconstruct return path; - i = route.size()-1; - for (; route[i] >= n_t; --i) - _returnPathIdx.push_back(i); - _returnPathIdx.push_back(i); // vertex of last transect - return true; } @@ -450,7 +486,13 @@ bool FlightPlan::_generateTransects(double lineDistance, double minTransectLengt { _transects.clear(); if (_scenario.getTilesENU().size() != _progress.size()){ - errorString = "Number of tiles is not equal to progress array length"; + ostringstream strstream; + strstream << "Number of tiles (" + << _scenario.getTilesENU().size() + << ") is not equal to progress array length (" + << _progress.size() + << ")"; + errorString = strstream.str(); return false; } @@ -573,10 +615,14 @@ void FlightPlan::_generateRoutingModel(const BoostLineString &vertices, FlightPlan::RoutingDataModel_t &dataModel, Matrix &graph) { +#ifdef SHOW_TIME auto start = std::chrono::high_resolution_clock::now(); +#endif graphFromPolygon(polygonOffset, vertices, graph); +#ifdef SHOW_TIME auto delta = std::chrono::duration_cast(std::chrono::high_resolution_clock::now()-start); cout << "Execution time graphFromPolygon(): " << delta.count() << " ms" << endl; +#endif // cout << endl; // for (size_t i=0; i &row = graph[i]; @@ -587,10 +633,14 @@ void FlightPlan::_generateRoutingModel(const BoostLineString &vertices, // } // cout << endl; Matrix distanceMatrix(graph); +#ifdef SHOW_TIME start = std::chrono::high_resolution_clock::now(); +#endif toDistanceMatrix(distanceMatrix); +#ifdef SHOW_TIME delta = std::chrono::duration_cast(std::chrono::high_resolution_clock::now()-start); cout << "Execution time toDistanceMatrix(): " << delta.count() << " ms" << endl; +#endif dataModel.distanceMatrix.setDimension(n0, n0); for (size_t i=0; i &getTiles() {return _tiles;} + const vector &getTileCenterPoints() {return _tileCenterPoints;} + const GeoPoint3D &getHomePositon() {return _homePosition;} + bool defined(double tileWidth, double tileHeight, double minTileArea); string errorString; @@ -78,8 +87,13 @@ namespace snake { vector _tilesENU; BoostLineString _tileCenterPointsENU; + vector _tiles; + vector _tileCenterPoints; + GeoPoint3D _geoOrigin; BoostPoint _homePositionENU; + + GeoPoint3D _homePosition; }; //======================================================================================== diff --git a/src/Wima/WimaAreaData.h b/src/Wima/WimaAreaData.h index 4c4898c61..97295f5fb 100644 --- a/src/Wima/WimaAreaData.h +++ b/src/Wima/WimaAreaData.h @@ -15,8 +15,6 @@ public: Q_PROPERTY(QString type READ type CONSTANT) WimaAreaData(QObject *parent = nullptr); - //WimaAreaData(const WimaAreaData &other, QObject *parent = nullptr); - //WimaAreaData(const WimaArea &other, QObject *parent = nullptr); WimaAreaData& operator=(const WimaAreaData& otherData) = delete; // avoid slicing double maxAltitude() const; @@ -25,8 +23,10 @@ public: QList coordinateList() const; bool containsCoordinate(const QGeoCoordinate &coordinate) const; + void append(const QGeoCoordinate &c) {_path.push_back(QVariant::fromValue(c));} + void clear() {_path.clear();} - virtual QString type() const = 0; + virtual QString type() const { return "Default";} signals: void maxAltitudeChanged (double maxAltitude); diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index b58384d30..afaef6c35 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -91,11 +91,6 @@ WimaController::WimaController(QObject *parent) connect(&_snakeWorker, &SnakeWorker::resultReady, this, &WimaController::_snakeStoreWorkerResults); } -QmlObjectListModel* WimaController::visualItems() -{ - return &_visualItems; -} - QStringList WimaController::loadNameFilters() const { QStringList filters; @@ -113,81 +108,6 @@ QStringList WimaController::saveNameFilters() const return filters; } -WimaDataContainer *WimaController::dataContainer() -{ - return _container; -} - -QmlObjectListModel *WimaController::missionItems() -{ - return &_missionItems; -} - -QmlObjectListModel *WimaController::currentMissionItems() -{ - return &_currentMissionItems; -} - -QVariantList WimaController::waypointPath() -{ - return _waypointPath; -} - -QVariantList WimaController::currentWaypointPath() -{ - return _currentWaypointPath; -} - -Fact *WimaController::enableWimaController() -{ - return &_enableWimaController; -} - -Fact *WimaController::overlapWaypoints() -{ - return &_overlapWaypoints; -} - -Fact *WimaController::maxWaypointsPerPhase() -{ - return &_maxWaypointsPerPhase; -} - -Fact *WimaController::showAllMissionItems() -{ - return &_showAllMissionItems; -} - -Fact *WimaController::showCurrentMissionItems() -{ - return &_showCurrentMissionItems; -} - -Fact *WimaController::flightSpeed() -{ - return &_flightSpeed; -} - -Fact *WimaController::arrivalReturnSpeed() -{ - return &_arrivalReturnSpeed; -} - -Fact *WimaController::altitude() -{ - return &_altitude; -} - -Fact *WimaController::reverse() -{ - return &_reverse; -} - -Fact *WimaController::enableSnake() -{ - return &_enableSnake; -} - bool WimaController::uploadOverrideRequired() const { return _uploadOverrideRequired; @@ -218,11 +138,6 @@ bool WimaController::snakeCalcInProgress() const return _snakeCalcInProgress; } -Fact *WimaController::startWaypointIndex() -{ - return &_nextPhaseStartWaypointIndex; -} - void WimaController::setMasterController(PlanMasterController *masterC) { _masterController = masterC; @@ -515,7 +430,7 @@ bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QVa */ bool WimaController::_fetchContainerData() { - // fetch only if valid, return true on sucess + // fetch only if valid, return true on success // reset visual items _visualItems.clear(); @@ -523,11 +438,13 @@ bool WimaController::_fetchContainerData() _currentMissionItems.clearAndDeleteContents(); _waypointPath.clear(); _currentWaypointPath.clear(); + _snakeTiles.clearAndDeleteContents(); emit visualItemsChanged(); emit missionItemsChanged(); emit currentMissionItemsChanged(); emit currentWaypointPathChanged(); + emit snakeTilesChanged(); _localPlanDataValid = false; @@ -643,11 +560,31 @@ bool WimaController::_fetchContainerData() corridor.geoPolygon.push_back(GeoPoint2D{c.latitude(), c.longitude()}); } corridor.type = AreaType::Corridor; + _scenario.addArea(mArea); _scenario.addArea(sArea); _scenario.addArea(corridor); - _verifyScenarioDefinedWithErrorMessage(); + // Check if scenario is defined. + if ( !_verifyScenarioDefinedWithErrorMessage() ) + return false; + + // Get tiles. + const auto &tiles = _scenario.getTiles(); + const auto &cps = _scenario.getTileCenterPoints(); + for ( int i=0; i < int(tiles.size()); ++i ) { + const auto &tile = tiles[i]; + WimaAreaData *QTile = new WimaAreaData(this); + for ( const auto &vertex : tile) { + QGeoCoordinate QVertex(vertex[0], vertex[1], vertex[2]); + QTile->append(QVertex); + } + const auto ¢erPoint = cps[i]; + QGeoCoordinate QCenterPoint(centerPoint[0], centerPoint[1], centerPoint[2]); + QTile->setCenter(QCenterPoint); + _snakeTiles.append(QTile); + } + emit visualItemsChanged(); emit missionItemsChanged(); @@ -759,7 +696,7 @@ bool WimaController::_calcNextPhase() // calculate path from last waypoint to home QVector returnPath; if ( !calcShortestPath(CSWpList.last(), _takeoffLandPostion, returnPath) ) { - qWarning("WimaController::calcNextPhase(): Not able to calc path from home to first waypoint."); + qWarning("WimaController::calcNextPhase(): Not able to calc path from home to last waypoint."); return false; } @@ -1039,8 +976,8 @@ void WimaController::_eventTimerHandler() _snakeWorker.setProgress(progress); _snakeWorker.setLineDistance(_snakeLineDistance.rawValue().toDouble()); _snakeWorker.setMinTransectLength(_snakeMinTransectLength.rawValue().toDouble()); - _snakeWorker.start(); _setSnakeCalcInProgress(true); + _snakeWorker.start(); } } } @@ -1316,8 +1253,10 @@ bool WimaController::_verifyScenarioDefinedWithErrorMessage() return value; } -void WimaController::_snakeStoreWorkerResults(const WorkerResult_t &r) -{ +void WimaController::_snakeStoreWorkerResults() +{ + _missionItems.clearAndDeleteContents(); + const WorkerResult_t &r = _snakeWorker.getResult(); _setSnakeCalcInProgress(false); if (!r.success) { qgcApp()->showMessage(r.errorMessage); @@ -1330,40 +1269,37 @@ void WimaController::_snakeStoreWorkerResults(const WorkerResult_t &r) QVector missionItems; missionItems.reserve(int(n)); - unsigned long startIdx = r.returnPathIdx.last(); + // Remove all items from mission controller. + _missionController->removeAll(); + QmlObjectListModel* missionControllerVisualItems = _missionController->visualItems(); + + // Create QVector containing all waypoints; + unsigned long startIdx = r.arrivalPathIdx.last(); unsigned long endIdx = r.returnPathIdx.first(); - QObject parent; - int seqNum = 0; + QVector waypoints; for (unsigned long i = startIdx; i <= endIdx; ++i) { QGeoCoordinate wp{r.waypoints[int(i)].value()}; - MissionItem item = new MissionItem(seqNum++, - MAV_CMD_NAV_WAYPOINT, - MAV_FRAME_GLOBAL_RELATIVE_ALT, - 0, // Hold time (delay for hover and capture to settle vehicle before image is taken) - 0.0, // No acceptance radius specified - 0.0, // Pass through waypoint - std::numeric_limits::quiet_NaN(), // Yaw unchanged - wp.latitude(), - wp.longitude(), - wp.altitude(), - true, // autoContinue - false, // isCurrentItem - &parent); + waypoints.append(wp); } - // create mission items - _missionController->removeAll(); - QmlObjectListModel* missionControllerVisualItems = _missionController->visualItems(); - // create SimpleMissionItem by using _missionController - for ( int i = 0; i < missionItems.size(); i++) { - _missionController->insertSimpleMissionItem(missionItems[i], missionControllerVisualItems->count()); + long insertIdx = missionControllerVisualItems->count(); + for (auto wp : waypoints) + _missionController->insertSimpleMissionItem(wp, insertIdx++); + SimpleMissionItem *takeOffItem = missionControllerVisualItems->value(1); + if (takeOffItem == nullptr) { + qWarning("WimaController::_snakeStoreWorkerResults(): Nullptr at SimpleMissionItem!"); + return; } + takeOffItem->setCommand(MAV_CMD_NAV_WAYPOINT); + takeOffItem->setCoordinate(waypoints[0]); + // copy mission items from _missionController to _missionItems + cout.precision(10); for ( int i = 1; i < missionControllerVisualItems->count(); i++) { SimpleMissionItem *visualItem = qobject_cast((*missionControllerVisualItems)[i]); if (visualItem == nullptr) { - qWarning("WimaController::fetchContainerData(): Nullptr at SimpleMissionItem!"); + qWarning("WimaController::_snakeStoreWorkerResults(): Nullptr at SimpleMissionItem!"); return; } SimpleMissionItem *visualItemCopy = new SimpleMissionItem(*visualItem, true, this); @@ -1378,8 +1314,8 @@ void WimaController::_snakeStoreWorkerResults(const WorkerResult_t &r) _nextPhaseStartWaypointIndex.setRawValue(reverse? _missionItems.count() : int(1)); connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::_calcNextPhase); - if(!_calcNextPhase()) - return; + _calcNextPhase(); + emit missionItemsChanged(); } void WimaController::_loadCurrentMissionItemsFromBuffer() diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index 1685bdb49..4d768ae9e 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -31,7 +31,7 @@ #define SMART_RTL_MAX_ATTEMPTS 3 // times #define SMART_RTL_ATTEMPT_INTERVAL 200 // ms #define EVENT_TIMER_INTERVAL 50 // ms -#define SNAKE_EVENT_LOOP_INTERVAL 50 // ms +#define SNAKE_EVENT_LOOP_INTERVAL 5000 // ms using namespace snake; @@ -86,39 +86,43 @@ public: Q_PROPERTY(Fact* snakeMinTileArea READ snakeMinTileArea CONSTANT) Q_PROPERTY(Fact* snakeLineDistance READ snakeLineDistance CONSTANT) Q_PROPERTY(Fact* snakeMinTransectLength READ snakeMinTransectLength CONSTANT) + Q_PROPERTY(QmlObjectListModel* snakeTiles READ snakeTiles NOTIFY snakeTilesChanged) // Property accessors - PlanMasterController* masterController (void) { return _masterController; } - MissionController* missionController (void) { return _missionController; } - QmlObjectListModel* visualItems (void); - QString currentFile (void) const { return _currentFile; } + PlanMasterController* masterController (void) { return _masterController; } + MissionController* missionController (void) { return _missionController; } + QmlObjectListModel* visualItems (void) { return &_visualItems; } + QString currentFile (void) const { return _currentFile; } QStringList loadNameFilters (void) const; QStringList saveNameFilters (void) const; - QString fileExtension (void) const { return wimaFileExtension; } + QString fileExtension (void) const { return wimaFileExtension; } QGCMapPolygon joinedArea (void) const; - WimaDataContainer* dataContainer (void); - QmlObjectListModel* missionItems (void); - QmlObjectListModel* currentMissionItems (void); - QVariantList waypointPath (void); - QVariantList currentWaypointPath (void); - Fact* enableWimaController (void); - Fact* overlapWaypoints (void); - Fact* maxWaypointsPerPhase (void); - Fact* startWaypointIndex (void); - Fact* showAllMissionItems (void); - Fact* showCurrentMissionItems(void); - Fact* flightSpeed (void); - Fact* arrivalReturnSpeed (void); - Fact* altitude (void); - Fact* reverse (void); - Fact* enableSnake (void); - Fact* snakeTileWidth (void) {return &_snakeTileWidth;} - Fact* snakeTileHeight (void) {return &_snakeTileHeight;} - Fact* snakeMinTileArea (void) {return &_snakeMinTileArea;} - Fact* snakeLineDistance (void) {return &_snakeLineDistance;} - Fact* snakeMinTransectLength (void) {return &_snakeMinTransectLength;} + WimaDataContainer* dataContainer (void) { return _container; } + QmlObjectListModel* missionItems (void) { return &_missionItems; } + QmlObjectListModel* currentMissionItems (void) { return &_currentMissionItems; } + QVariantList waypointPath (void) { return _waypointPath; } + QVariantList currentWaypointPath (void) { return _currentWaypointPath; } + Fact* enableWimaController (void) { return &_enableWimaController; } + Fact* overlapWaypoints (void) { return &_overlapWaypoints; } + Fact* maxWaypointsPerPhase (void) { return &_maxWaypointsPerPhase; } + Fact* startWaypointIndex (void) { return &_nextPhaseStartWaypointIndex; } + Fact* showAllMissionItems (void) { return &_showAllMissionItems; } + Fact* showCurrentMissionItems(void) { return &_showCurrentMissionItems; } + Fact* flightSpeed (void) { return &_flightSpeed; } + Fact* arrivalReturnSpeed (void) { return &_arrivalReturnSpeed; }; + Fact* altitude (void) { return &_altitude; } + Fact* reverse (void) { return &_reverse; } + + Fact* enableSnake (void) { return &_enableSnake; } + Fact* snakeTileWidth (void) { return &_snakeTileWidth;} + Fact* snakeTileHeight (void) { return &_snakeTileHeight;} + Fact* snakeMinTileArea (void) { return &_snakeMinTileArea;} + Fact* snakeLineDistance (void) { return &_snakeLineDistance;} + Fact* snakeMinTransectLength (void) { return &_snakeMinTransectLength;} + QmlObjectListModel* snakeTiles (void) { return &_snakeTiles;} + bool uploadOverrideRequired (void) const; double phaseDistance (void) const; double phaseDuration (void) const; @@ -208,6 +212,7 @@ signals: void returnUserRequestConfirmRequired (void); void snakeConnectionStatusChanged (void); void snakeCalcInProgressChanged (void); + void snakeTilesChanged (void); private: enum SRTL_Reason {BatteryLow, UserRequest}; private slots: @@ -232,7 +237,7 @@ private slots: void _setSnakeCalcInProgress (bool inProgress); bool _verifyScenarioDefined (void); bool _verifyScenarioDefinedWithErrorMessage (void); - void _snakeStoreWorkerResults (const WorkerResult_t &r); + void _snakeStoreWorkerResults (); private: diff --git a/src/Wima/WimaControllerDetail.cc b/src/Wima/WimaControllerDetail.cc index 626c02b6f..9f50497e8 100644 --- a/src/Wima/WimaControllerDetail.cc +++ b/src/Wima/WimaControllerDetail.cc @@ -16,9 +16,9 @@ void SnakeWorker::setScenario(const Scenario &scenario) void SnakeWorker::setProgress(const QList &progress) { + _progress.clear(); for (auto p : progress) { - assert(p >= -1); - assert(p <= 100); + assert(p >= -1 && p <= 100); _progress.push_back(p); } } @@ -35,15 +35,20 @@ void SnakeWorker::setMinTransectLength(double minTransectLength) _minTransectLength = minTransectLength; } +const WorkerResult_t &SnakeWorker::getResult() const +{ + return _result; +} + void SnakeWorker::run() { - WorkerResult_t result{QVariantList{}, - QVector{}, - QVector{}, - false, - QString{}}; + // Reset _result struct. + _result.success = false; + _result.errorMessage = ""; + _result.waypoints.clear(); + _result.returnPathIdx.clear(); + _result.arrivalPathIdx.clear(); - // partial success FlightPlan flightplan; flightplan.setScenario(_scenario); flightplan.setProgress(_progress); @@ -52,28 +57,29 @@ void SnakeWorker::run() if ( !flightplan.generate(_lineDistance, _minTransectLength) ){ // error for (auto c : flightplan.errorString){ - result.errorMessage.push_back(QChar(c)); + _result.errorMessage.push_back(QChar(c)); } } else { //success!!! + _result.success = true; // Fill result struct. auto waypoints = flightplan.getWaypoints(); for (auto vertex : waypoints){ - result.waypoints.append(QVariant::fromValue(QGeoCoordinate(vertex[0], vertex[1], 0))); + _result.waypoints.append(QVariant::fromValue(QGeoCoordinate(vertex[0], vertex[1], 0))); } auto arrivalPathIdx = flightplan.getArrivalPathIdx(); for (auto idx : arrivalPathIdx){ - result.arrivalPathIdx.append(idx); + _result.arrivalPathIdx.append(idx); } auto returnPathIdx = flightplan.getReturnPathIdx(); for (auto idx : returnPathIdx){ - result.returnPathIdx.append(idx); + _result.returnPathIdx.append(idx); } } - emit resultReady(result); + emit resultReady(); } diff --git a/src/Wima/WimaControllerDetail.h b/src/Wima/WimaControllerDetail.h index 0bb57ed0b..ed23a3849 100644 --- a/src/Wima/WimaControllerDetail.h +++ b/src/Wima/WimaControllerDetail.h @@ -34,8 +34,10 @@ public: void setLineDistance (double lineDistance); void setMinTransectLength (double minTransectLength); + const WorkerResult_t &getResult() const; + signals: - void resultReady(const WorkerResult_t &result); + void resultReady(); protected: void run() override; @@ -45,5 +47,6 @@ private: vector _progress; double _lineDistance; double _minTransectLength; + WorkerResult_t _result; }; diff --git a/src/Wima/WimaPlaner.cc b/src/Wima/WimaPlaner.cc index ab744092a..57ee5a2e9 100644 --- a/src/Wima/WimaPlaner.cc +++ b/src/Wima/WimaPlaner.cc @@ -33,7 +33,7 @@ WimaPlaner::WimaPlaner(QObject *parent) connect(&_updateTimer, &QTimer::timeout, this, &WimaPlaner::updateTimerSlot); connect(this, &WimaPlaner::joinedAreaValidChanged, this, &WimaPlaner::updateMission); - _updateTimer.setInterval(500); // 250 ms means: max update time 2*250 ms + _updateTimer.setInterval(300); // 300 ms means: max update time 2*300 ms _updateTimer.start(); // for debugging and testing purpose, remove if not needed anymore @@ -445,8 +445,10 @@ bool WimaPlaner::loadFromFile(const QString &filename) } //if (_circularSurvey == nullptr) - recalcJoinedArea(); - updateMission(); + if ( !recalcJoinedArea() ) + return false; + if ( !updateMission() ) + return false; // remove temporary file if ( !temporaryFile.remove() ){ @@ -770,8 +772,9 @@ void WimaPlaner::updateTimerSlot() // measurementArea if (_measurementAreaChanging) { if (_measurementArea.path() == _lastMeasurementAreaPath) { // is it still changing? - recalcJoinedArea(); - calcArrivalAndReturnPath(); + setReadyForSync(false); + if ( recalcJoinedArea() && calcArrivalAndReturnPath() ) + setReadyForSync(true); _measurementAreaChanging = false; } } else { @@ -782,8 +785,9 @@ void WimaPlaner::updateTimerSlot() // corridor if (_corridorChanging) { if (_corridor.path() == _lastCorridorPath) { // is it still changing? - recalcJoinedArea(); - calcArrivalAndReturnPath(); + setReadyForSync(false); + if ( recalcJoinedArea() && calcArrivalAndReturnPath() ) + setReadyForSync(true); _corridorChanging = false; } } else { @@ -794,8 +798,9 @@ void WimaPlaner::updateTimerSlot() // service area if (_serviceAreaChanging) { if (_serviceArea.path() == _lastServiceAreaPath) { // is it still changing? - recalcJoinedArea(); - calcArrivalAndReturnPath(); + setReadyForSync(false); + if ( recalcJoinedArea() && calcArrivalAndReturnPath() ) + setReadyForSync(true); _serviceAreaChanging = false; } } else { diff --git a/src/comm/utilities.h b/src/comm/utilities.h index fd5df8675..07989c52e 100644 --- a/src/comm/utilities.h +++ b/src/comm/utilities.h @@ -48,13 +48,13 @@ public: //! \return True if the event is ready to be executed, false either. //! //! This function must be called by a user provided timer interrupt handler. A internal counter increases with every call. - //! If timerInterval <= tickInerval*counter the counter gets reset and the function returns true. If the condition is not + //! If _tickInerval <= _timerInterval*_tickCount the counter gets reset and the function returns true. If the condition is not //! met the function returns false. //! bool ready(void) { assert(_isInitialized == true); _tickCount++; - bool ready = _timerInterval <= _tickInerval*_tickCount; + bool ready = _tickInerval <= _timerInterval*_tickCount; if (ready) _tickCount = 0; return ready; diff --git a/src/main.cc b/src/main.cc index 386e2d00c..e7296039e 100644 --- a/src/main.cc +++ b/src/main.cc @@ -148,7 +148,7 @@ int main(int argc, char *argv[]) // install the message handler AppMessages::installHandler(); -QList + #ifdef Q_OS_MAC #ifndef __ios__ // Prevent Apple's app nap from screwing us over @@ -253,8 +253,8 @@ QList // on in the code. qRegisterMetaType > >(); - // Snake - qRegisterMetaType("WorkerResult"); + // Snake (Is this the right place for the following statement?) + //qRegisterMetaType("WorkerResult"); app->_initCommon(); //-- Initialize Cache System -- 2.22.0