From b37dda84c2ec00738dddca3124a388aaa759df12 Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Wed, 26 Jun 2019 18:05:45 +0200 Subject: [PATCH] editing WimaController ofter repurposeing --- src/Wima/WimaController.cc | 486 +-------------------------- src/Wima/WimaController.h | 56 +--- src/Wima/WimaDataContainer.cc | 9 + src/Wima/WimaDataContainer.h | 4 + src/Wima/WimaPlaner.cc | 611 +++++++++++++++++++++++++++++++++- src/Wima/WimaPlaner.h | 135 +++++++- 6 files changed, 774 insertions(+), 527 deletions(-) diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index 7f9ef2399..370b1ef1f 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -7,14 +7,13 @@ const char* WimaController::missionItemsName = "MissionItems"; WimaController::WimaController(QObject *parent) : QObject (parent) , _readyForSaveSend (false) - , _currentPolygonIndex (-1) , _container (nullptr) , _joinedArea (this) , _opArea (this) , _serArea (this) , _corridor (this) { - connect(this, &WimaController::currentPolygonIndexChanged, this, &WimaController::recalcPolygonInteractivity); + } QmlObjectListModel* WimaController::visualItems() @@ -44,7 +43,7 @@ QGeoCoordinate WimaController::joinedAreaCenter() const return _joinedArea.center(); } -WimaArea WimaController::joinedArea() const +QGCMapPolygon WimaController::joinedArea() const { return _joinedArea; } @@ -61,19 +60,11 @@ void WimaController::setMissionController(MissionController *missionC) emit missionControllerChanged(); } -void WimaController::setCurrentPolygonIndex(int index) -{ - if(index >= 0 && index < _visualItems.count() && index != _currentPolygonIndex){ - _currentPolygonIndex = index; - - emit currentPolygonIndexChanged(index); - } -} - void WimaController::setDataContainer(WimaDataContainer *container) { if (_container == nullptr && container != nullptr) { _container = container; + pullFromContainer(); emit dataContainerChanged(); } @@ -84,99 +75,6 @@ void WimaController::startWimaController(bool flyView) } -void WimaController::removeArea(int index) -{ - if(index >= 0 && index < _visualItems.count()){ - WimaArea* area = qobject_cast(_visualItems.removeAt(index)); - - if ( area == nullptr) { - qWarning("WimaController::removeArea(): nullptr catched, internal error."); - return; - } - area->clear(); - - emit visualItemsChanged(); - - if (_visualItems.count() == 0) { - // this branch is reached if all items are removed - // to guarentee proper behavior, _currentPolygonIndex must be set to a invalid value, as on constructor init. - _currentPolygonIndex = -1; - return; - } - - if(_currentPolygonIndex >= _visualItems.count()){ - setCurrentPolygonIndex(_visualItems.count() - 1); - }else{ - recalcPolygonInteractivity(_currentPolygonIndex); - } - }else{ - qWarning("Index out of bounds!"); - } - -} - -bool WimaController::addGOperationArea() -{ - if (!_visualItems.contains(&_opArea)) { - _visualItems.append(&_opArea); - - int newIndex = _visualItems.count()-1; - setCurrentPolygonIndex(newIndex); - - emit visualItemsChanged(); - return true; - } else { - return false; - } -} - -bool WimaController::addServiceArea() -{ - if (!_visualItems.contains(&_serArea)) { - _visualItems.append(&_serArea); - - int newIndex = _visualItems.count()-1; - setCurrentPolygonIndex(newIndex); - - emit visualItemsChanged(); - return true; - } else { - return false; - } -} - -bool WimaController::addVehicleCorridor() -{ - if (!_visualItems.contains(&_corridor)) { - _visualItems.append(&_corridor); - - int newIndex = _visualItems.count()-1; - setCurrentPolygonIndex(newIndex); - - emit visualItemsChanged(); - return true; - } else { - return false; - } -} - -void WimaController::removeAll() -{ - bool changesApplied = false; - while (_visualItems.count() > 0) { - removeArea(0); - changesApplied = true; - } - - _missionController->removeAll(); - - _currentFile = ""; - - emit currentFileChanged(); - if ( changesApplied ) - emit visualItemsChanged(); -} - void WimaController::startMission() { @@ -199,407 +97,33 @@ void WimaController::resumeMission() bool WimaController::updateMission() { - - setReadyForSaveSend(false); - #define debug 0 - - if ( !recalcJoinedArea()) { - qgcApp()->showMessage(tr("Not able to join areas. Areas must be overlapping")); - return false; - } - - #if debug - _visualItems.append(&_joinedArea); - #endif - - // reset visual items - _missionController->removeAll(); - QmlObjectListModel* missionItems = _missionController->visualItems(); - // set home position to serArea center - MissionSettingsItem* settingsItem= qobject_cast(missionItems->get(0)); - if (settingsItem == nullptr){ - qWarning("WimaController::updateMission(): settingsItem == nullptr"); - return false; - } - - // set altitudes, temporary measure to solve bugs - QGeoCoordinate center = _serArea.center(); - center.setAltitude(0); - _serArea.setCenter(center); - center = _opArea.center(); - center.setAltitude(0); - _opArea.setCenter(center); - center = _corridor.center(); - center.setAltitude(0); - _corridor.setCenter(center); - - - // set HomePos. to serArea center - settingsItem->setCoordinate(_serArea.center()); - - // create take off position item - int sequenceNumber = _missionController->insertSimpleMissionItem(_serArea.center(), missionItems->count()); - _missionController->setCurrentPlanViewIndex(sequenceNumber, true); - - - // create survey item, will be extened with more()-> mission types in the future - _missionController->insertComplexMissionItem(_missionController->surveyComplexItemName(), _opArea.center(), missionItems->count()); - SurveyComplexItem* survey = qobject_cast(missionItems->get(missionItems->count()-1)); - if (survey == nullptr){ - qWarning("WimaController::updateMission(): survey == nullptr"); - return false; - } else { - survey->surveyAreaPolygon()->clear(); - survey->surveyAreaPolygon()->appendVertices(_opArea.coordinateList()); - //survey-> - } - - // calculate path from take off to opArea - QGeoCoordinate start = _serArea.center(); - QGeoCoordinate end = survey->visualTransectPoints().first().value(); - QList path; - if ( !WimaArea::dijkstraPath(start, end, _joinedArea, path)) { - qgcApp()->showMessage(tr("Not able to calculate the path from takeoff position to measurement area.")); - return false; - } - for (int i = 1; i < path.count()-1; i++) { - sequenceNumber = _missionController->insertSimpleMissionItem(path.value(i), missionItems->count()-1); - _missionController->setCurrentPlanViewIndex(sequenceNumber, true); - } - - // calculate return path - start = survey->visualTransectPoints().last().value(); - end = _serArea.center(); - path.clear(); - if ( ! WimaArea::dijkstraPath(start, end, _joinedArea, path)) { - qgcApp()->showMessage(tr("Not able to calculate the path from measurement area to landing position.")); - return false; - } - for (int i = 1; i < path.count()-1; i++) { - sequenceNumber = _missionController->insertSimpleMissionItem(path.value(i), missionItems->count()); - _missionController->setCurrentPlanViewIndex(sequenceNumber, true); - } - - // create land position item - sequenceNumber = _missionController->insertSimpleMissionItem(_serArea.center(), missionItems->count()); - _missionController->setCurrentPlanViewIndex(sequenceNumber, true); - SimpleMissionItem* landItem = qobject_cast(missionItems->get(missionItems->count()-1)); - if (landItem == nullptr){ - qWarning("WimaController::updateMission(): landItem == nullptr"); - return false; - } else { - Vehicle* controllerVehicle = _masterController->controllerVehicle(); - MAV_CMD landCmd = controllerVehicle->vtol() ? MAV_CMD_NAV_VTOL_LAND : MAV_CMD_NAV_LAND; - if (controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains(landCmd)) { - landItem->setCommand(landCmd); - } - } - - updateContainer(); - setReadyForSaveSend(true); return true; } void WimaController::saveToCurrent() { - saveToFile(_currentFile); } void WimaController::saveToFile(const QString& filename) { - if (filename.isEmpty()) { - return; - } - QString planFilename = filename; - if (!QFileInfo(filename).fileName().contains(".")) { - planFilename += QString(".%1").arg(wimaFileExtension); - } - - QFile file(planFilename); - if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) { - qgcApp()->showMessage(tr("Plan save error %1 : %2").arg(filename).arg(file.errorString())); - _currentFile.clear(); - emit currentFileChanged(); - } else { - FileType fileType = FileType::WimaFile; - if ( planFilename.contains(QString(".%1").arg(wimaFileExtension)) ) { - fileType = FileType::WimaFile; - } else if ( planFilename.contains(QString(".%1").arg(AppSettings::planFileExtension)) ) { - fileType = FileType::PlanFile; - } else { - if ( planFilename.contains(".") ) { - qgcApp()->showMessage(tr("File format not supported")); - } else { - qgcApp()->showMessage(tr("File without file extension not accepted.")); - return; - } - } - - QJsonDocument saveDoc = saveToJson(fileType); - file.write(saveDoc.toJson()); - if(_currentFile != planFilename) { - _currentFile = planFilename; - emit currentFileChanged(); - } - } } bool WimaController::loadFromCurrent() { - return loadFromFile(_currentFile); + return true; } bool WimaController::loadFromFile(const QString &filename) { - #define debug 0 - QString errorString; - QString errorMessage = tr("Error loading Plan file (%1). %2").arg(filename).arg("%1"); - - if (filename.isEmpty()) { - return false; - } - - QFileInfo fileInfo(filename); - QFile file(filename); - - if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) { - errorString = file.errorString() + QStringLiteral(" ") + filename; - qgcApp()->showMessage(errorMessage.arg(errorString)); - return false; - } - - if(fileInfo.suffix() == wimaFileExtension) { - QJsonDocument jsonDoc; - QByteArray bytes = file.readAll(); - - if (!JsonHelper::isJsonFile(bytes, jsonDoc, errorString)) { - qgcApp()->showMessage(errorMessage.arg(errorString)); - return false; - } - - QJsonObject json = jsonDoc.object(); - // AreaItems - QJsonArray areaArray = json[areaItemsName].toArray(); - _visualItems.clear(); - - int validAreaCounter = 0; - for( int i = 0; i < areaArray.size() && validAreaCounter < 3; i++) { - QJsonObject jsonArea = areaArray[i].toObject(); - - if (jsonArea.contains(WimaArea::areaTypeName) && jsonArea[WimaArea::areaTypeName].isString()) { - if ( jsonArea[WimaArea::areaTypeName] == WimaGOperationArea::wimaGOperationAreaName) { - print(_opArea); - bool success = _opArea.loadFromJson(jsonArea, errorString); - print(_opArea); - - if ( !success ) { - qgcApp()->showMessage(errorMessage.arg(errorString)); - return false; - } - - validAreaCounter++; - _visualItems.append(&_opArea); - emit visualItemsChanged(); - } else if ( jsonArea[WimaArea::areaTypeName] == WimaServiceArea::wimaServiceAreaName) { - bool success = _serArea.loadFromJson(jsonArea, errorString); - - if ( !success ) { - qgcApp()->showMessage(errorMessage.arg(errorString)); - return false; - } - - validAreaCounter++; - _visualItems.append(&_serArea); - emit visualItemsChanged(); - } else if ( jsonArea[WimaArea::areaTypeName] == WimaVCorridor::wimaVCorridorName) { - bool success = _corridor.loadFromJson(jsonArea, errorString); - - if ( !success ) { - qgcApp()->showMessage(errorMessage.arg(errorString)); - return false; - } - - validAreaCounter++; - _visualItems.append(&_corridor); - emit visualItemsChanged(); - } else { - errorString += QString(tr("%s not supported.\n").arg(WimaArea::areaTypeName)); - qgcApp()->showMessage(errorMessage.arg(errorString)); - return false; - } - } else { - errorString += QString(tr("Invalid or non existing entry for %s.\n").arg(WimaArea::areaTypeName)); - return false; - } - } - - _currentFile.sprintf("%s/%s.%s", fileInfo.path().toLocal8Bit().data(), fileInfo.completeBaseName().toLocal8Bit().data(), wimaFileExtension); - - emit currentFileChanged(); - recalcJoinedArea(); - - // MissionItems - // extrac MissionItems part - QJsonDocument missionJsonDoc = QJsonDocument(json[missionItemsName].toObject()); - // create temporary file with missionItems - QFile temporaryFile; - QString cropedFileName = filename.section("/",0,-2); - #if debug - qWarning() << cropedFileName; - #endif - QString temporaryFileName; - for (int i = 0; ; i++) { - temporaryFileName = cropedFileName.append("/temp%1.%2").arg(i).arg(AppSettings::planFileExtension); - - if ( !QFile::exists(temporaryFileName) ) { - temporaryFile.setFileName(temporaryFileName); - if ( temporaryFile.open(QIODevice::WriteOnly | QIODevice::Text) ) { - break; - } - } - - if ( i > 1000) { - qWarning("WimaController::loadFromFile(): not able to create temporary file."); - return false; - } - } - - temporaryFile.write(missionJsonDoc.toJson()); - - // load from temporary file - _masterController->loadFromFile(temporaryFileName); - - // remove temporary file - if ( !temporaryFile.remove() ){ - qWarning("WimaController::loadFromFile(): not able to remove temporary file."); - } - - return true; - - } else if ( fileInfo.suffix() == AppSettings::planFileExtension ){ - _masterController->loadFromFile(filename); - - return true;// might be wrong return value - } else { - errorString += QString(tr("File extension not supported.\n")); - qgcApp()->showMessage(errorMessage.arg(errorString)); - - return false; - } -} - -void WimaController::recalcPolygonInteractivity(int index) -{ - if (index >= 0 && index < _visualItems.count()) { - resetAllInteractive(); - WimaArea* interactivePoly = qobject_cast(_visualItems.get(index)); - interactivePoly->setInteractive(true); - } -} - -bool WimaController::recalcJoinedArea() -{ - // join service area, op area and corridor - _joinedArea = _serArea; - _joinedArea.join(_corridor); - - if ( !_joinedArea.join(_opArea) ) - return false; // this happens if all areas are pairwise disjoint - else { - emit joinedAreaChanged() ; - return true; - } - - -} - -void WimaController::updateContainer() -{ - // Sets the pointers (inside _container) to the areas (of this). - // Should be called only (once) after a _container has been assigned. - if (_container != nullptr) { - _container->setOpArea(&_opArea); - _container->setSerArea(&_serArea); - _container->setCorridor(&_corridor); - _container->setJoinedArea(&_joinedArea); - } else { - qWarning("WimaController::uploadToContainer(): no container assigned."); - } + return true; } -void WimaController::resetAllInteractive() -{ - // Marks all areas as inactive (area.interactive == false) - int itemCount = _visualItems.count(); - if (itemCount > 0){ - for (int i = 0; i < itemCount; i++) { - WimaArea* iteratorPoly = qobject_cast(_visualItems.get(i)); - iteratorPoly->setInteractive(false); - } - } -} -void WimaController::setInteractive() -{ - recalcPolygonInteractivity(_currentPolygonIndex); -} QJsonDocument WimaController::saveToJson(FileType fileType) { - /// This function save all areas (of WimaController) and all mission items (of MissionController) to a QJsonDocument - /// @param fileType is either WimaFile or PlanFile (enum), if fileType == PlanFile only mission items are stored - QJsonObject json; - - if ( fileType == FileType::WimaFile ) { - QJsonArray jsonArray; - - for (int i = 0; i < _visualItems.count(); i++) { - QJsonObject json; - - WimaArea* area = qobject_cast(_visualItems.get(i)); - - if (area == nullptr) { - qWarning("WimaController::saveToJson(): Internal error, area == nullptr!"); - return QJsonDocument(); - } - - // check the type of area, create and append the JsonObject to the JsonArray once determined - WimaGOperationArea* opArea = qobject_cast(area); - if (opArea != nullptr) { - opArea->saveToJson(json); - jsonArray.append(json); - continue; - } - - WimaServiceArea* serArea = qobject_cast(area); - if (serArea != nullptr) { - serArea->saveToJson(json); - jsonArray.append(json); - continue; - } - - WimaVCorridor* corridor = qobject_cast(area); - if (corridor != nullptr) { - corridor->saveToJson(json); - jsonArray.append(json); - continue; - } - - // if non of the obove branches was trigger, type must be WimaArea - area->saveToJson(json); - jsonArray.append(json); - } - - json[areaItemsName] = jsonArray; - json[missionItemsName] = _masterController->saveToJson().object(); - - return QJsonDocument(json); - } else if (fileType == FileType::PlanFile) { - return _masterController->saveToJson(); - } - return QJsonDocument(json); } void WimaController::setReadyForSaveSend(bool ready) diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index f241a3215..3c8af8af8 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -32,14 +32,12 @@ public: Q_PROPERTY(PlanMasterController* masterController READ masterController WRITE setMasterController NOTIFY masterControllerChanged) Q_PROPERTY(MissionController* missionController READ missionController WRITE setMissionController NOTIFY missionControllerChanged) Q_PROPERTY(QmlObjectListModel* visualItems READ visualItems NOTIFY visualItemsChanged) - Q_PROPERTY(int currentPolygonIndex READ currentPolygonIndex WRITE setCurrentPolygonIndex NOTIFY currentPolygonIndexChanged) Q_PROPERTY(QString currentFile READ currentFile NOTIFY currentFileChanged) Q_PROPERTY(QStringList loadNameFilters READ loadNameFilters CONSTANT) Q_PROPERTY(QStringList saveNameFilters READ saveNameFilters CONSTANT) Q_PROPERTY(QString fileExtension READ fileExtension CONSTANT) Q_PROPERTY(QGeoCoordinate joinedAreaCenter READ joinedAreaCenter CONSTANT) Q_PROPERTY(WimaArea joinedArea READ joinedArea NOTIFY joinedAreaChanged) - Q_PROPERTY(bool flyView READ flyView CONSTANT) Q_PROPERTY(WimaDataContainer* dataContainer WRITE setDataContainer NOTIFY dataContainerChanged) Q_PROPERTY(bool readyForSaveSend READ readyForSaveSend NOTIFY readyForSaveSendChanged) @@ -48,13 +46,12 @@ public: PlanMasterController* masterController (void) const { return _masterController; } MissionController* missionController (void) const { return _missionController; } QmlObjectListModel* visualItems (void) ; - int currentPolygonIndex (void) const { return _currentPolygonIndex; } QString currentFile (void) const { return _currentFile; } QStringList loadNameFilters (void) const; QStringList saveNameFilters (void) const; QString fileExtension (void) const { return wimaFileExtension; } QGeoCoordinate joinedAreaCenter (void) const; - WimaArea joinedArea (void) const; + QGCMapPolygon joinedArea(void) const; bool readyForSaveSend (void) const { return _readyForSaveSend; } @@ -62,35 +59,20 @@ public: // Property setters void setMasterController (PlanMasterController* masterController); void setMissionController (MissionController* missionController); - /// Sets the integer index pointing to the current polygon. Current polygon is set interactive. - void setCurrentPolygonIndex (int index); void setDataContainer (WimaDataContainer* container); // Member Methodes Q_INVOKABLE void startWimaController(bool flyView); - Q_INVOKABLE bool addGOperationArea(); - /// Removes an area from _visualItems - /// @param index Index of the area to be removed - Q_INVOKABLE void removeArea(int index); - Q_INVOKABLE bool addServiceArea(); - Q_INVOKABLE bool addVehicleCorridor(); - /// Remove all areas from WimaController and all mission items from MissionController - Q_INVOKABLE void removeAll(); - Q_INVOKABLE void startMission(); Q_INVOKABLE void abortMission(); Q_INVOKABLE void pauseMission(); Q_INVOKABLE void resumeMission(); - /// Recalculates vehicle corridor, flight path, etc. Q_INVOKABLE bool updateMission(); - Q_INVOKABLE void saveToCurrent(); - Q_INVOKABLE void saveToFile(const QString& filename); + Q_INVOKABLE void saveToCurrent (); + Q_INVOKABLE void saveToFile (const QString& filename); Q_INVOKABLE bool loadFromCurrent(); - Q_INVOKABLE bool loadFromFile(const QString& filename); - - Q_INVOKABLE void resetAllInteractive(void); - Q_INVOKABLE void setInteractive(void); + Q_INVOKABLE bool loadFromFile (const QString& filename); // static Members @@ -102,32 +84,28 @@ public: QJsonDocument saveToJson(FileType fileType); void setReadyForSaveSend(bool ready); + signals: void masterControllerChanged (void); void missionControllerChanged (void); void visualItemsChanged (void); - void currentPolygonIndexChanged (int index); void currentFileChanged (); - void joinedAreaChanged (); void dataContainerChanged (); void readyForSaveSendChanged (bool ready); private slots: - void recalcPolygonInteractivity (int index); - bool recalcJoinedArea (); - void updateContainer ();// only executed if flyView == false - void downloadFromContainer ();// only executed if flyView == true + void pullFromContainer (); private: - bool _readyForSaveSend; - PlanMasterController *_masterController; - MissionController *_missionController; - int _currentPolygonIndex; - QString _currentFile; - WimaDataContainer *_container; - QmlObjectListModel _visualItems; - WimaArea _joinedArea; - WimaGOperationArea _opArea; - WimaServiceArea _serArea; - WimaVCorridor _corridor; + bool _readyForSaveSend; // basically true if updateMission() was sucessful + PlanMasterController *_masterController; + MissionController *_missionController; + QString _currentFile; // file for saveing + WimaDataContainer *_container; // container for data exchange with WimaController + QmlObjectListModel _visualItems; // contains all visible areas + // The following areas are of type QGCMapPolygon (only path information is required, as they are used for visualisation) + QGCMapPolygon _joinedArea; // joined area fromed by opArea, serArea, _corridor + QGCMapPolygon _opArea; // measurement area + QGCMapPolygon _serArea; // area for supplying + QGCMapPolygon _corridor; // corridor connecting opArea and serArea }; diff --git a/src/Wima/WimaDataContainer.cc b/src/Wima/WimaDataContainer.cc index 075ee370e..60d69cbd2 100644 --- a/src/Wima/WimaDataContainer.cc +++ b/src/Wima/WimaDataContainer.cc @@ -45,3 +45,12 @@ void WimaDataContainer::setCorridor(const WimaVCorridor *corridor) emit corridorChanged(_corridor); } } + +void WimaDataContainer::setVisualItems(const QmlObjectListModel *visualItems) +{ + if (_visualItems != visualItems) { + _visualItems = visualItems; + + emit visualItemsChanged(_visualItems); + } +} diff --git a/src/Wima/WimaDataContainer.h b/src/Wima/WimaDataContainer.h index 41ffa99b2..4dbca1d54 100644 --- a/src/Wima/WimaDataContainer.h +++ b/src/Wima/WimaDataContainer.h @@ -21,23 +21,27 @@ public: const WimaGOperationArea * opArea (void) { return _opArea; } const WimaServiceArea * serArea (void) { return _serArea; } const WimaVCorridor * corridor (void) { return _corridor; } + const QmlObjectListModel * visualItems (void) { return _visualItems; } signals: void joinedAreaChanged (const WimaArea *area); void opAreaChanged (const WimaGOperationArea *area); void serAreaChanged (const WimaServiceArea *area); void corridorChanged (const WimaVCorridor *area); + void visualItemsChanged (const QmlObjectListModel *area); public slots: void setJoinedArea (const WimaArea *joinedArea); void setOpArea (const WimaGOperationArea *opArea); void setSerArea (const WimaServiceArea *serArea); void setCorridor (const WimaVCorridor *corridor); + void setVisualItems (const QmlObjectListModel *visualItems); private: const WimaArea *_joinedArea; const WimaGOperationArea *_opArea; const WimaServiceArea *_serArea; const WimaVCorridor *_corridor; + const QmlObjectListModel *_visualItems; }; diff --git a/src/Wima/WimaPlaner.cc b/src/Wima/WimaPlaner.cc index bbd65e63d..b1ad63835 100644 --- a/src/Wima/WimaPlaner.cc +++ b/src/Wima/WimaPlaner.cc @@ -1,6 +1,615 @@ #include "WimaPlaner.h" -WimaPlaner::WimaPlaner() +const char* WimaPlaner::wimaFileExtension = "wima"; +const char* WimaPlaner::areaItemsName = "AreaItems"; +const char* WimaPlaner::missionItemsName = "MissionItems"; + +WimaPlaner::WimaPlaner(QObject *parent) + : QObject (parent) + , _readyForSaveSend (false) + , _currentAreaIndex (-1) + , _container (nullptr) + , _joinedArea (this) + , _opArea (this) + , _serArea (this) + , _corridor (this) +{ + connect(this, &WimaPlaner::currentPolygonIndexChanged, this, &WimaPlaner::recalcPolygonInteractivity); +} + +QmlObjectListModel* WimaPlaner::visualItems() +{ + return &_visualItems; +} + +QStringList WimaPlaner::loadNameFilters() const +{ + QStringList filters; + + filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension) << + tr("All Files (*.*)"); + return filters; +} + +QStringList WimaPlaner::saveNameFilters() const +{ + QStringList filters; + + filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension); + return filters; +} + +QGeoCoordinate WimaPlaner::joinedAreaCenter() const +{ + return _joinedArea.center(); +} + +WimaArea WimaPlaner::joinedArea() const +{ + return _joinedArea; +} + +void WimaPlaner::setMasterController(PlanMasterController *masterC) +{ + _masterController = masterC; + emit masterControllerChanged(); +} + +void WimaPlaner::setMissionController(MissionController *missionC) +{ + _missionController = missionC; + emit missionControllerChanged(); +} + +void WimaPlaner::setCurrentPolygonIndex(int index) +{ + if(index >= 0 && index < _visualItems.count() && index != _currentAreaIndex){ + _currentAreaIndex = index; + + emit currentPolygonIndexChanged(index); + } +} + +void WimaPlaner::setDataContainer(WimaDataContainer *container) +{ + if (_container == nullptr && container != nullptr) { + _container = container; + + emit dataContainerChanged(); + } +} + +void WimaPlaner::startWimaPlaner(bool flyView) +{ + +} + +void WimaPlaner::removeArea(int index) +{ + if(index >= 0 && index < _visualItems.count()){ + WimaArea* area = qobject_cast(_visualItems.removeAt(index)); + + if ( area == nullptr) { + qWarning("WimaPlaner::removeArea(): nullptr catched, internal error."); + return; + } + area->clear(); + + emit visualItemsChanged(); + + if (_visualItems.count() == 0) { + // this branch is reached if all items are removed + // to guarentee proper behavior, _currentAreaIndex must be set to a invalid value, as on constructor init. + _currentAreaIndex = -1; + return; + } + + if(_currentAreaIndex >= _visualItems.count()){ + setCurrentPolygonIndex(_visualItems.count() - 1); + }else{ + recalcPolygonInteractivity(_currentAreaIndex); + } + }else{ + qWarning("Index out of bounds!"); + } + +} + +bool WimaPlaner::addGOperationArea() +{ + if (!_visualItems.contains(&_opArea)) { + _visualItems.append(&_opArea); + + int newIndex = _visualItems.count()-1; + setCurrentPolygonIndex(newIndex); + + emit visualItemsChanged(); + return true; + } else { + return false; + } +} + +bool WimaPlaner::addServiceArea() +{ + if (!_visualItems.contains(&_serArea)) { + _visualItems.append(&_serArea); + + int newIndex = _visualItems.count()-1; + setCurrentPolygonIndex(newIndex); + + emit visualItemsChanged(); + return true; + } else { + return false; + } +} + +bool WimaPlaner::addVehicleCorridor() +{ + if (!_visualItems.contains(&_corridor)) { + _visualItems.append(&_corridor); + + int newIndex = _visualItems.count()-1; + setCurrentPolygonIndex(newIndex); + + emit visualItemsChanged(); + return true; + } else { + return false; + } +} + +void WimaPlaner::removeAll() +{ + bool changesApplied = false; + while (_visualItems.count() > 0) { + removeArea(0); + changesApplied = true; + } + + _missionController->removeAll(); + + _currentFile = ""; + + emit currentFileChanged(); + if ( changesApplied ) + emit visualItemsChanged(); +} + +void WimaPlaner::startMission() +{ + +} + +void WimaPlaner::abortMission() +{ + +} + +void WimaPlaner::pauseMission() +{ + +} + +void WimaPlaner::resumeMission() +{ + +} + +bool WimaPlaner::updateMission() +{ + + setReadyForSaveSend(false); + #define debug 0 + + if ( !recalcJoinedArea()) { + qgcApp()->showMessage(tr("Not able to join areas. Areas must be overlapping")); + return false; + } + + #if debug + _visualItems.append(&_joinedArea); + #endif + + // reset visual items + _missionController->removeAll(); + QmlObjectListModel* missionItems = _missionController->visualItems(); + // set home position to serArea center + MissionSettingsItem* settingsItem= qobject_cast(missionItems->get(0)); + if (settingsItem == nullptr){ + qWarning("WimaPlaner::updateMission(): settingsItem == nullptr"); + return false; + } + + // set altitudes, temporary measure to solve bugs + QGeoCoordinate center = _serArea.center(); + center.setAltitude(0); + _serArea.setCenter(center); + center = _opArea.center(); + center.setAltitude(0); + _opArea.setCenter(center); + center = _corridor.center(); + center.setAltitude(0); + _corridor.setCenter(center); + + + // set HomePos. to serArea center + settingsItem->setCoordinate(_serArea.center()); + + // create take off position item + int sequenceNumber = _missionController->insertSimpleMissionItem(_serArea.center(), missionItems->count()); + _missionController->setCurrentPlanViewIndex(sequenceNumber, true); + + + // create survey item, will be extened with more()-> mission types in the future + _missionController->insertComplexMissionItem(_missionController->surveyComplexItemName(), _opArea.center(), missionItems->count()); + SurveyComplexItem* survey = qobject_cast(missionItems->get(missionItems->count()-1)); + if (survey == nullptr){ + qWarning("WimaPlaner::updateMission(): survey == nullptr"); + return false; + } else { + survey->surveyAreaPolygon()->clear(); + survey->surveyAreaPolygon()->appendVertices(_opArea.coordinateList()); + //survey-> + } + + // calculate path from take off to opArea + QGeoCoordinate start = _serArea.center(); + QGeoCoordinate end = survey->visualTransectPoints().first().value(); + QList path; + if ( !WimaArea::dijkstraPath(start, end, _joinedArea, path)) { + qgcApp()->showMessage(tr("Not able to calculate the path from takeoff position to measurement area.")); + return false; + } + for (int i = 1; i < path.count()-1; i++) { + sequenceNumber = _missionController->insertSimpleMissionItem(path.value(i), missionItems->count()-1); + _missionController->setCurrentPlanViewIndex(sequenceNumber, true); + } + + // calculate return path + start = survey->visualTransectPoints().last().value(); + end = _serArea.center(); + path.clear(); + if ( ! WimaArea::dijkstraPath(start, end, _joinedArea, path)) { + qgcApp()->showMessage(tr("Not able to calculate the path from measurement area to landing position.")); + return false; + } + for (int i = 1; i < path.count()-1; i++) { + sequenceNumber = _missionController->insertSimpleMissionItem(path.value(i), missionItems->count()); + _missionController->setCurrentPlanViewIndex(sequenceNumber, true); + } + + // create land position item + sequenceNumber = _missionController->insertSimpleMissionItem(_serArea.center(), missionItems->count()); + _missionController->setCurrentPlanViewIndex(sequenceNumber, true); + SimpleMissionItem* landItem = qobject_cast(missionItems->get(missionItems->count()-1)); + if (landItem == nullptr){ + qWarning("WimaPlaner::updateMission(): landItem == nullptr"); + return false; + } else { + Vehicle* controllerVehicle = _masterController->controllerVehicle(); + MAV_CMD landCmd = controllerVehicle->vtol() ? MAV_CMD_NAV_VTOL_LAND : MAV_CMD_NAV_LAND; + if (controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains(landCmd)) { + landItem->setCommand(landCmd); + } + } + + pushToContainer(); + setReadyForSaveSend(true); + return true; +} + +void WimaPlaner::saveToCurrent() +{ + saveToFile(_currentFile); +} + +void WimaPlaner::saveToFile(const QString& filename) +{ + if (filename.isEmpty()) { + return; + } + + QString planFilename = filename; + if (!QFileInfo(filename).fileName().contains(".")) { + planFilename += QString(".%1").arg(wimaFileExtension); + } + + QFile file(planFilename); + if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) { + qgcApp()->showMessage(tr("Plan save error %1 : %2").arg(filename).arg(file.errorString())); + _currentFile.clear(); + emit currentFileChanged(); + } else { + FileType fileType = FileType::WimaFile; + if ( planFilename.contains(QString(".%1").arg(wimaFileExtension)) ) { + fileType = FileType::WimaFile; + } else if ( planFilename.contains(QString(".%1").arg(AppSettings::planFileExtension)) ) { + fileType = FileType::PlanFile; + } else { + if ( planFilename.contains(".") ) { + qgcApp()->showMessage(tr("File format not supported")); + } else { + qgcApp()->showMessage(tr("File without file extension not accepted.")); + return; + } + } + + QJsonDocument saveDoc = saveToJson(fileType); + file.write(saveDoc.toJson()); + if(_currentFile != planFilename) { + _currentFile = planFilename; + emit currentFileChanged(); + } + } +} + +bool WimaPlaner::loadFromCurrent() +{ + return loadFromFile(_currentFile); +} + +bool WimaPlaner::loadFromFile(const QString &filename) +{ + #define debug 0 + QString errorString; + QString errorMessage = tr("Error loading Plan file (%1). %2").arg(filename).arg("%1"); + + if (filename.isEmpty()) { + return false; + } + + QFileInfo fileInfo(filename); + QFile file(filename); + + if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) { + errorString = file.errorString() + QStringLiteral(" ") + filename; + qgcApp()->showMessage(errorMessage.arg(errorString)); + return false; + } + + if(fileInfo.suffix() == wimaFileExtension) { + QJsonDocument jsonDoc; + QByteArray bytes = file.readAll(); + + if (!JsonHelper::isJsonFile(bytes, jsonDoc, errorString)) { + qgcApp()->showMessage(errorMessage.arg(errorString)); + return false; + } + + QJsonObject json = jsonDoc.object(); + // AreaItems + QJsonArray areaArray = json[areaItemsName].toArray(); + _visualItems.clear(); + + int validAreaCounter = 0; + for( int i = 0; i < areaArray.size() && validAreaCounter < 3; i++) { + QJsonObject jsonArea = areaArray[i].toObject(); + + if (jsonArea.contains(WimaArea::areaTypeName) && jsonArea[WimaArea::areaTypeName].isString()) { + if ( jsonArea[WimaArea::areaTypeName] == WimaGOperationArea::wimaGOperationAreaName) { + print(_opArea); + bool success = _opArea.loadFromJson(jsonArea, errorString); + print(_opArea); + + if ( !success ) { + qgcApp()->showMessage(errorMessage.arg(errorString)); + return false; + } + + validAreaCounter++; + _visualItems.append(&_opArea); + emit visualItemsChanged(); + } else if ( jsonArea[WimaArea::areaTypeName] == WimaServiceArea::wimaServiceAreaName) { + bool success = _serArea.loadFromJson(jsonArea, errorString); + + if ( !success ) { + qgcApp()->showMessage(errorMessage.arg(errorString)); + return false; + } + + validAreaCounter++; + _visualItems.append(&_serArea); + emit visualItemsChanged(); + } else if ( jsonArea[WimaArea::areaTypeName] == WimaVCorridor::wimaVCorridorName) { + bool success = _corridor.loadFromJson(jsonArea, errorString); + + if ( !success ) { + qgcApp()->showMessage(errorMessage.arg(errorString)); + return false; + } + + validAreaCounter++; + _visualItems.append(&_corridor); + emit visualItemsChanged(); + } else { + errorString += QString(tr("%s not supported.\n").arg(WimaArea::areaTypeName)); + qgcApp()->showMessage(errorMessage.arg(errorString)); + return false; + } + } else { + errorString += QString(tr("Invalid or non existing entry for %s.\n").arg(WimaArea::areaTypeName)); + return false; + } + } + + _currentFile.sprintf("%s/%s.%s", fileInfo.path().toLocal8Bit().data(), fileInfo.completeBaseName().toLocal8Bit().data(), wimaFileExtension); + + emit currentFileChanged(); + recalcJoinedArea(); + + // MissionItems + // extrac MissionItems part + QJsonDocument missionJsonDoc = QJsonDocument(json[missionItemsName].toObject()); + // create temporary file with missionItems + QFile temporaryFile; + QString cropedFileName = filename.section("/",0,-2); + #if debug + qWarning() << cropedFileName; + #endif + QString temporaryFileName; + for (int i = 0; ; i++) { + temporaryFileName = cropedFileName.append("/temp%1.%2").arg(i).arg(AppSettings::planFileExtension); + + if ( !QFile::exists(temporaryFileName) ) { + temporaryFile.setFileName(temporaryFileName); + if ( temporaryFile.open(QIODevice::WriteOnly | QIODevice::Text) ) { + break; + } + } + + if ( i > 1000) { + qWarning("WimaPlaner::loadFromFile(): not able to create temporary file."); + return false; + } + } + + temporaryFile.write(missionJsonDoc.toJson()); + + // load from temporary file + _masterController->loadFromFile(temporaryFileName); + + // remove temporary file + if ( !temporaryFile.remove() ){ + qWarning("WimaPlaner::loadFromFile(): not able to remove temporary file."); + } + + return true; + + } else if ( fileInfo.suffix() == AppSettings::planFileExtension ){ + _masterController->loadFromFile(filename); + + return true;// might be wrong return value + } else { + errorString += QString(tr("File extension not supported.\n")); + qgcApp()->showMessage(errorMessage.arg(errorString)); + + return false; + } +} + +void WimaPlaner::recalcPolygonInteractivity(int index) +{ + if (index >= 0 && index < _visualItems.count()) { + resetAllInteractive(); + WimaArea* interactivePoly = qobject_cast(_visualItems.get(index)); + interactivePoly->setInteractive(true); + } +} + +bool WimaPlaner::recalcJoinedArea() +{ + // join service area, op area and corridor + _joinedArea = _serArea; + _joinedArea.join(_corridor); + + if ( !_joinedArea.join(_opArea) ) + return false; // this happens if all areas are pairwise disjoint + else { + emit joinedAreaChanged() ; + return true; + } + + +} + +void WimaPlaner::pushToContainer() +{ + // Sets the pointers (inside _container) to the areas (of this). + // Should be called only (once) after a _container has been assigned. + if (_container != nullptr) { + _container->setOpArea(&_opArea); + _container->setSerArea(&_serArea); + _container->setCorridor(&_corridor); + _container->setJoinedArea(&_joinedArea); + _container->setVisualItems(&_visualItems); + } else { + qWarning("WimaPlaner::uploadToContainer(): no container assigned."); + } +} + +void WimaPlaner::resetAllInteractive() { + // Marks all areas as inactive (area.interactive == false) + int itemCount = _visualItems.count(); + if (itemCount > 0){ + for (int i = 0; i < itemCount; i++) { + WimaArea* iteratorPoly = qobject_cast(_visualItems.get(i)); + iteratorPoly->setInteractive(false); + } + } +} + +void WimaPlaner::setInteractive() +{ + recalcPolygonInteractivity(_currentAreaIndex); +} + +QJsonDocument WimaPlaner::saveToJson(FileType fileType) +{ + /// This function save all areas (of WimaPlaner) and all mission items (of MissionController) to a QJsonDocument + /// @param fileType is either WimaFile or PlanFile (enum), if fileType == PlanFile only mission items are stored + QJsonObject json; + + if ( fileType == FileType::WimaFile ) { + QJsonArray jsonArray; + + for (int i = 0; i < _visualItems.count(); i++) { + QJsonObject json; + + WimaArea* area = qobject_cast(_visualItems.get(i)); + + if (area == nullptr) { + qWarning("WimaPlaner::saveToJson(): Internal error, area == nullptr!"); + return QJsonDocument(); + } + + // check the type of area, create and append the JsonObject to the JsonArray once determined + WimaGOperationArea* opArea = qobject_cast(area); + if (opArea != nullptr) { + opArea->saveToJson(json); + jsonArray.append(json); + continue; + } + WimaServiceArea* serArea = qobject_cast(area); + if (serArea != nullptr) { + serArea->saveToJson(json); + jsonArray.append(json); + continue; + } + + WimaVCorridor* corridor = qobject_cast(area); + if (corridor != nullptr) { + corridor->saveToJson(json); + jsonArray.append(json); + continue; + } + + // if non of the obove branches was trigger, type must be WimaArea + area->saveToJson(json); + jsonArray.append(json); + } + + json[areaItemsName] = jsonArray; + json[missionItemsName] = _masterController->saveToJson().object(); + + return QJsonDocument(json); + } else if (fileType == FileType::PlanFile) { + return _masterController->saveToJson(); + } + + return QJsonDocument(json); } + +void WimaPlaner::setReadyForSaveSend(bool ready) +{ + if (ready != _readyForSaveSend) { + _readyForSaveSend = ready; + emit readyForSaveSendChanged(ready); + } +} + + + diff --git a/src/Wima/WimaPlaner.h b/src/Wima/WimaPlaner.h index 7bed0f495..3a003e3c6 100644 --- a/src/Wima/WimaPlaner.h +++ b/src/Wima/WimaPlaner.h @@ -1,11 +1,134 @@ -#ifndef WIMAPLANER_H -#define WIMAPLANER_H +#pragma once +#include +#include "QGCMapPolygon.h" +#include "QmlObjectListModel.h" -class WimaPlaner +#include "WimaArea.h" +#include "WimaGOperationArea.h" +#include "WimaServiceArea.h" +#include "WimaVCorridor.h" +#include "WimaDataContainer.h" + +#include "PlanMasterController.h" +#include "MissionController.h" +#include "SurveyComplexItem.h" +#include "SimpleMissionItem.h" +#include "MissionSettingsItem.h" +#include "JsonHelper.h" +#include "QGCApplication.h" + + +class WimaPlaner : public QObject { + Q_OBJECT + + enum FileType {WimaFile, PlanFile}; + public: - WimaPlaner(); -}; + WimaPlaner(QObject *parent = nullptr); + template + WimaPlaner(T t, QObject *parent = nullptr) = delete; + template + WimaPlaner(T t) = delete; + + Q_PROPERTY(PlanMasterController* masterController READ masterController WRITE setMasterController NOTIFY masterControllerChanged) + Q_PROPERTY(MissionController* missionController READ missionController WRITE setMissionController NOTIFY missionControllerChanged) + Q_PROPERTY(QmlObjectListModel* visualItems READ visualItems NOTIFY visualItemsChanged) + Q_PROPERTY(int currentPolygonIndex READ currentPolygonIndex WRITE setCurrentPolygonIndex NOTIFY currentPolygonIndexChanged) + Q_PROPERTY(QString currentFile READ currentFile NOTIFY currentFileChanged) + Q_PROPERTY(QStringList loadNameFilters READ loadNameFilters CONSTANT) + Q_PROPERTY(QStringList saveNameFilters READ saveNameFilters CONSTANT) + Q_PROPERTY(QString fileExtension READ fileExtension CONSTANT) + Q_PROPERTY(QGeoCoordinate joinedAreaCenter READ joinedAreaCenter CONSTANT) + Q_PROPERTY(WimaArea joinedArea READ joinedArea NOTIFY joinedAreaChanged) + Q_PROPERTY(WimaDataContainer* dataContainer WRITE setDataContainer NOTIFY dataContainerChanged) + Q_PROPERTY(bool readyForSaveSend READ readyForSaveSend NOTIFY readyForSaveSendChanged) + + + // Property accessors + PlanMasterController* masterController (void) const { return _masterController; } + MissionController* missionController (void) const { return _missionController; } + QmlObjectListModel* visualItems (void) ; + int currentPolygonIndex (void) const { return _currentAreaIndex; } + QString currentFile (void) const { return _currentFile; } + QStringList loadNameFilters (void) const; + QStringList saveNameFilters (void) const; + QString fileExtension (void) const { return wimaFileExtension; } + QGeoCoordinate joinedAreaCenter (void) const; + WimaArea joinedArea (void) const; + bool readyForSaveSend (void) const { return _readyForSaveSend; } + + -#endif // WIMAPLANER_H \ No newline at end of file + // Property setters + void setMasterController (PlanMasterController* masterController); + void setMissionController (MissionController* missionController); + /// Sets the integer index pointing to the current polygon. Current polygon is set interactive. + void setCurrentPolygonIndex (int index); + void setDataContainer (WimaDataContainer* container); + + // Member Methodes + Q_INVOKABLE void startWimaPlaner(bool flyView); + Q_INVOKABLE bool addGOperationArea(); + /// Removes an area from _visualItems + /// @param index Index of the area to be removed + Q_INVOKABLE void removeArea(int index); + Q_INVOKABLE bool addServiceArea(); + Q_INVOKABLE bool addVehicleCorridor(); + /// Remove all areas from WimaPlaner and all mission items from MissionController + Q_INVOKABLE void removeAll(); + + Q_INVOKABLE void startMission(); + Q_INVOKABLE void abortMission(); + Q_INVOKABLE void pauseMission(); + Q_INVOKABLE void resumeMission(); + /// Recalculates vehicle corridor, flight path, etc. + Q_INVOKABLE bool updateMission(); + + Q_INVOKABLE void saveToCurrent(); + Q_INVOKABLE void saveToFile(const QString& filename); + Q_INVOKABLE bool loadFromCurrent(); + Q_INVOKABLE bool loadFromFile(const QString& filename); + + Q_INVOKABLE void resetAllInteractive(void); + Q_INVOKABLE void setInteractive(void); + + + // static Members + static const char* wimaFileExtension; + static const char* areaItemsName; + static const char* missionItemsName; + + // Member Methodes + QJsonDocument saveToJson(FileType fileType); + void setReadyForSaveSend(bool ready); + +signals: + void masterControllerChanged (void); + void missionControllerChanged (void); + void visualItemsChanged (void); + void currentPolygonIndexChanged (int index); + void currentFileChanged (); + void joinedAreaChanged (); + void dataContainerChanged (); + void readyForSaveSendChanged (bool ready); + +private slots: + void recalcPolygonInteractivity (int index); + bool recalcJoinedArea (); + void pushToContainer (); + +private: + bool _readyForSaveSend; // basically true if updateMission() was sucessful + PlanMasterController *_masterController; + MissionController *_missionController; + int _currentAreaIndex; + QString _currentFile; // file for saveing + WimaDataContainer *_container; // container for data exchange with WimaController + QmlObjectListModel _visualItems; // contains all visible areas + WimaArea _joinedArea; // joined area fromed by opArea, serArea, _corridor + WimaGOperationArea _opArea; // measurement area + WimaServiceArea _serArea; // area for supplying + WimaVCorridor _corridor; // corridor connecting opArea and serArea +}; -- 2.22.0