#include "WimaPlaner.h" #include "MissionController.h" #include "MissionSettingsItem.h" #include "PlanMasterController.h" #include "QGCApplication.h" #include "QGCMapPolygon.h" #include "SimpleMissionItem.h" #include "Geometry/GeoUtilities.h" #include "Geometry/PlanimetryCalculus.h" #include "OptimisationTools.h" #include "CircularSurvey.h" #include "Geometry/WimaArea.h" #include "Geometry/WimaAreaData.h" #include "WimaBridge.h" const char *WimaPlaner::wimaFileExtension = "wima"; const char *WimaPlaner::areaItemsName = "AreaItems"; const char *WimaPlaner::missionItemsName = "MissionItems"; WimaPlaner::WimaPlaner(QObject *parent) : QObject(parent), _masterController(nullptr), _missionController(nullptr), _currentAreaIndex(-1), _wimaBridge(nullptr), _mAreaChanged(true), _sAreaChanged(true), _corridorChanged(true), _joinedArea(this), _arrivalPathLength(0), _returnPathLength(0), _TSComplexItem(nullptr), _surveyChanged(true), _synchronized(false), _needsUpdate(true) { connect(this, &WimaPlaner::currentPolygonIndexChanged, this, &WimaPlaner::updatePolygonInteractivity); connect(&this->_measurementArea, &WimaArea::pathChanged, [this] { this->_mAreaChanged = true; this->setNeedsUpdate(true); }); connect(&this->_serviceArea, &WimaArea::pathChanged, [this] { this->_sAreaChanged = true; this->setNeedsUpdate(true); }); connect(&this->_serviceArea, &WimaServiceArea::depotChanged, [this] { this->_sAreaChanged = true; this->setNeedsUpdate(true); }); connect(&this->_corridor, &WimaArea::pathChanged, [this] { this->_corridorChanged = true; this->setNeedsUpdate(true); }); #ifndef NDEBUG // for debugging and testing purpose, remove if not needed anymore connect(&_autoLoadTimer, &QTimer::timeout, this, &WimaPlaner::autoLoadMission); _autoLoadTimer.setSingleShot(true); _autoLoadTimer.start(300); #endif } PlanMasterController *WimaPlaner::masterController() { return _masterController; } MissionController *WimaPlaner::missionController() { return _missionController; } QmlObjectListModel *WimaPlaner::visualItems() { return &_visualItems; } int WimaPlaner::currentPolygonIndex() const { return _currentAreaIndex; } QString WimaPlaner::currentFile() const { return _currentFile; } 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; } QString WimaPlaner::fileExtension() const { return wimaFileExtension; } QGeoCoordinate WimaPlaner::joinedAreaCenter() const { return _joinedArea.center(); } WimaBridge *WimaPlaner::wimaBridge() { return _wimaBridge; } 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::setWimaBridge(WimaBridge *bridge) { if (bridge != nullptr) { _wimaBridge = bridge; emit wimaBridgeChanged(); } } bool WimaPlaner::synchronized() { return _synchronized; } bool WimaPlaner::needsUpdate() { return _needsUpdate; } WimaPlaner *WimaPlaner::thisPointer() { return this; } 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(); area->borderPolygon()->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. resetAllInteractive(); _currentAreaIndex = -1; return; } if (_currentAreaIndex >= _visualItems.count()) { setCurrentPolygonIndex(_visualItems.count() - 1); } else { updatePolygonInteractivity(_currentAreaIndex); } } else { qWarning("Index out of bounds!"); } } bool WimaPlaner::addMeasurementArea() { if (!_visualItems.contains(&_measurementArea)) { _visualItems.append(&_measurementArea); int newIndex = _visualItems.count() - 1; setCurrentPolygonIndex(newIndex); emit visualItemsChanged(); return true; } else { return false; } } bool WimaPlaner::addServiceArea() { if (!_visualItems.contains(&_serviceArea)) { _visualItems.append(&_serviceArea); int newIndex = _visualItems.count() - 1; setCurrentPolygonIndex(newIndex); emit visualItemsChanged(); return true; } else { return false; } } bool WimaPlaner::addCorridor() { 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 = ""; _TSComplexItem = nullptr; emit currentFileChanged(); if (changesApplied) emit visualItemsChanged(); } bool WimaPlaner::update() { this->_synchronized = false; emit synchronizedChanged(); // ====================== update joined area ====================== // check if at least service area and measurement area are available if (_visualItems.indexOf(&_serviceArea) == -1 || _visualItems.indexOf(&_measurementArea) == -1) return false; // Check if polygons have at least three vertices if (_serviceArea.count() < 3) { qgcApp()->showMessage(tr("Service area has less than three vertices.")); return false; } if (_measurementArea.count() < 3) { qgcApp()->showMessage(tr("Measurement area has less than three vertices.")); return false; } // Check for simple polygons if (!_serviceArea.isSimplePolygon()) { qgcApp()->showMessage(tr("Service area is not a simple polygon. " "Only simple polygons allowed.\n")); return false; } if (!_corridor.isSimplePolygon() && _corridor.count() > 0) { qgcApp()->showMessage(tr("Corridor is not a simple polygon. Only " "simple polygons allowed.\n")); return false; } if (!_measurementArea.isSimplePolygon()) { qgcApp()->showMessage(tr("Measurement area is not a simple " "polygon. Only simple polygons allowed.\n")); return false; } if (!_serviceArea.containsCoordinate(_serviceArea.depot())) { qgcApp()->showMessage(tr("Depot not inside service area.")); return false; } // Join areas. bool jAreaChanged = this->_mAreaChanged || this->_sAreaChanged || this->_corridorChanged; if (jAreaChanged) { _joinedArea.setPath(_serviceArea.path()); if (_corridor.count() >= 3) { _joinedArea.join(_corridor); } if (!_joinedArea.join(_measurementArea)) { qgcApp()->showMessage( tr("Not able to join areas. Service and measurement area" " must be overlapping, or connected through a " "corridor.")); return false; } } // =========================================================== // ====================== update survey ====================== bool updateSurveyArea = jAreaChanged; // extract old survey data QmlObjectListModel *missionItems = _missionController->visualItems(); int surveyIndex = missionItems->indexOf(_TSComplexItem); // create survey item if not yet present if (surveyIndex == -1) { _missionController->insertComplexMissionItem( _missionController->circularSurveyComplexItemName(), _measurementArea.center(), missionItems->count()); _TSComplexItem = qobject_cast( missionItems->get(missionItems->count() - 1)); if (_TSComplexItem == nullptr) { qWarning("WimaPlaner::update(): survey == nullptr"); return false; } // establish connections _TSComplexItem->setRefPoint(_measurementArea.center()); _TSComplexItem->setHidePolygon(true); connect(_TSComplexItem, &CircularSurvey::missionItemReady, [this] { this->_surveyChanged = true; this->setNeedsUpdate(true); }); updateSurveyArea = true; } if (updateSurveyArea) { // update survey area _TSComplexItem->surveyAreaPolygon()->clear(); _TSComplexItem->surveyAreaPolygon()->appendVertices( _measurementArea.coordinateList()); _TSComplexItem->setDepot(this->_serviceArea.depot()); _TSComplexItem->setSafeArea(this->_joinedArea.coordinateList()); this->_mAreaChanged = false; this->_sAreaChanged = false; this->_corridorChanged = false; } else if (this->_surveyChanged) { // ========================================================== // ====================== update paths ====================== surveyIndex = missionItems->indexOf(_TSComplexItem); if (surveyIndex == -1) { qWarning("WimaPlaner::update(): no survey item"); return false; } // remove old arrival and return path int size = missionItems->count(); for (int i = surveyIndex + 1; i < size; i++) _missionController->removeMissionItem(surveyIndex + 1); for (int i = surveyIndex - 1; i > 1; i--) _missionController->removeMissionItem(i); // set home position to serArea center MissionSettingsItem *settingsItem = qobject_cast(missionItems->get(0)); if (settingsItem == nullptr) { qWarning("WimaPlaner::update(): settingsItem == nullptr"); return false; } // set altitudes QGeoCoordinate point = _serviceArea.depot(); point.setAltitude(0); _serviceArea.setDepot(point); point = _measurementArea.center(); point.setAltitude(0); _measurementArea.setCenter(point); point = _corridor.center(); point.setAltitude(0); _corridor.setCenter(point); settingsItem->setCoordinate(_serviceArea.depot()); // set takeoff position bool setCommandNeeded = false; if (missionItems->count() < 3) { setCommandNeeded = true; _missionController->insertSimpleMissionItem(_serviceArea.depot(), 1); } SimpleMissionItem *takeOffItem = qobject_cast(missionItems->get(1)); if (takeOffItem == nullptr) { qWarning("WimaPlaner::update(): takeOffItem == nullptr"); return false; } if (setCommandNeeded) _missionController->setTakeoffCommand(*takeOffItem); takeOffItem->setCoordinate(_serviceArea.depot()); if (_TSComplexItem->visualTransectPoints().size() == 0) { qWarning("WimaPlaner::update(): survey no points."); return false; } // calculate path from take off to survey QGeoCoordinate start = _serviceArea.depot(); QGeoCoordinate end = _TSComplexItem->coordinate(); #ifdef QT_DEBUG // if (!_visualItems.contains(&_joinedArea)) //_visualItems.append(&_joinedArea); #endif QVector path; if (!shortestPath(start, end, path)) { qgcApp()->showMessage(QString(tr("Not able to calculate path from " "takeoff position to measurement area.")) .toLocal8Bit() .data()); return false; } _arrivalPathLength = path.size() - 1; int sequenceNumber = 0; for (int i = 1; i < path.count() - 1; i++) { sequenceNumber = _missionController->insertSimpleMissionItem( path[i], missionItems->count() - 1); _missionController->setCurrentPlanViewIndex(sequenceNumber, true); } // calculate return path start = _TSComplexItem->exitCoordinate(); end = _serviceArea.depot(); path.clear(); if (!shortestPath(start, end, path)) { qgcApp()->showMessage(QString(tr("Not able to calculate the path from " "measurement area to landing position.")) .toLocal8Bit() .data()); return false; } _returnPathLength = path.size() - 1; // -1: fist item is last measurement point for (int i = 1; i < path.count() - 1; i++) { sequenceNumber = _missionController->insertSimpleMissionItem( path[i], missionItems->count()); _missionController->setCurrentPlanViewIndex(sequenceNumber, true); } // create land position item sequenceNumber = _missionController->insertSimpleMissionItem( _serviceArea.depot(), missionItems->count()); _missionController->setCurrentPlanViewIndex(sequenceNumber, true); SimpleMissionItem *landItem = qobject_cast( missionItems->get(missionItems->count() - 1)); if (landItem == nullptr) { qWarning("WimaPlaner::calcArrivalAndReturnPath(): landItem == nullptr"); return false; } else { if (!_missionController->setLandCommand(*landItem)) return false; } this->_surveyChanged = false; setNeedsUpdate(false); } 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] == WimaMeasurementArea::WimaMeasurementAreaName) { bool success = _measurementArea.loadFromJson(jsonArea, errorString); if (!success) { qgcApp()->showMessage(errorMessage.arg(errorString)); return false; } validAreaCounter++; _visualItems.append(&_measurementArea); emit visualItemsChanged(); } else if (jsonArea[WimaArea::areaTypeName] == WimaServiceArea::wimaServiceAreaName) { bool success = _serviceArea.loadFromJson(jsonArea, errorString); if (!success) { qgcApp()->showMessage(errorMessage.arg(errorString)); return false; } validAreaCounter++; _visualItems.append(&_serviceArea); emit visualItemsChanged(); } else if (jsonArea[WimaArea::areaTypeName] == WimaCorridor::WimaCorridorName) { 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(); QJsonObject missionObject = json[missionItemsName].toObject(); // qWarning() << json[missionItemsName].type(); QJsonDocument missionJsonDoc = QJsonDocument(missionObject); // 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 + QString("/temp%1.%2").arg(i).arg(AppSettings::planFileExtension); // qWarning() << temporaryFileName; 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; } } // qWarning() << missionJsonDoc.toVariant().toString(); temporaryFile.write(missionJsonDoc.toJson()); temporaryFile.close(); // load from temporary file _masterController->loadFromFile(temporaryFileName); QmlObjectListModel *missionItems = _missionController->visualItems(); _TSComplexItem = nullptr; for (int i = 0; i < missionItems->count(); i++) { _TSComplexItem = missionItems->value(i); if (_TSComplexItem != nullptr) { _TSComplexItem->setHidePolygon(true); connect(_TSComplexItem, &CircularSurvey::missionItemReady, [this] { this->_surveyChanged = true; this->setNeedsUpdate(true); }); break; } } if (!update()) return false; // remove temporary file if (!temporaryFile.remove()) { qWarning( "WimaPlaner::loadFromFile(): not able to remove temporary file."); } this->_synchronized = false; emit synchronizedChanged(); 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::updatePolygonInteractivity(int index) { if (index >= 0 && index < _visualItems.count()) { resetAllInteractive(); WimaArea *interactivePoly = qobject_cast(_visualItems.get(index)); if (interactivePoly != nullptr) interactivePoly->setWimaAreaInteractive(true); } } void WimaPlaner::synchronize() { if (_wimaBridge != nullptr) { if (readyForSynchronization()) { auto planData = toPlanData(); (void)_wimaBridge->setWimaPlanData(planData); this->_synchronized = true; emit synchronizedChanged(); } } else { qWarning("WimaPlaner::uploadToContainer(): no container assigned."); } } bool WimaPlaner::readyForSynchronization() { return !_needsUpdate && _measurementArea.ready(); } bool WimaPlaner::shortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector &path) { using namespace GeoUtilities; using namespace PolygonCalculus; QPolygonF polygon2D; toCartesianList(_joinedArea.coordinateList(), /*origin*/ start, polygon2D); QPointF start2D(0, 0); QPointF end2D; QVector path2D; toCartesian(destination, start, end2D); bool retVal = PolygonCalculus::shortestPath(polygon2D, start2D, end2D, path2D); toGeoList(path2D, /*origin*/ start, path); return retVal; } void WimaPlaner::setNeedsUpdate(bool needsUpdate) { if (this->_needsUpdate != needsUpdate) { this->_needsUpdate = needsUpdate; emit needsUpdateChanged(); } } 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->setWimaAreaInteractive(false); } } } void WimaPlaner::setInteractive() { updatePolygonInteractivity(_currentAreaIndex); } /*! * \fn WimaPlanData WimaPlaner::toPlanData() * * Returns a \c WimaPlanData object containing information about the current * mission. The \c WimaPlanData object holds only the data which is relevant * for the \c WimaController class. Should only be called if update() was * successful. * * \sa WimaController, WimaPlanData */ QSharedPointer WimaPlaner::toPlanData() { QSharedPointer planData(new WimaPlanData()); // store areas planData->append(WimaMeasurementAreaData(_measurementArea)); planData->append(WimaServiceAreaData(_serviceArea)); planData->append(WimaCorridorData(_corridor)); planData->append(WimaJoinedAreaData(_joinedArea)); // convert mission items to mavlink commands QList missionItems; if (_missionController && _missionController->visualItems()) { int surveyIndex = _missionController->visualItems()->indexOf(_TSComplexItem); if (surveyIndex > 0) { _TSComplexItem->appendMissionItems(missionItems, nullptr); } } // store mavlink commands planData->append(missionItems); return planData; } #ifndef NDEBUG void WimaPlaner::autoLoadMission() { loadFromFile("/home/valentin/Desktop/drones/qgroundcontrol/Paths/" "KlingenbachTest.wima"); synchronize(); } #endif 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 WimaMeasurementArea *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; } WimaCorridor *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); }