#include "WimaPlaner.h" #include "CircularSurveyComplexItem.h" const char* WimaPlaner::wimaFileExtension = "wima"; const char* WimaPlaner::areaItemsName = "AreaItems"; const char* WimaPlaner::missionItemsName = "MissionItems"; WimaPlaner::WimaPlaner(QObject *parent) : QObject (parent) , _missionReady (false) , _currentAreaIndex (-1) , _container (nullptr) , _joinedArea (this) , _measurementArea (this) , _serviceArea (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(); } 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) { if (_container != nullptr) { disconnect(this, &WimaPlaner::missionReadyChanged, _container, &WimaDataContainer::newDataAvailable); } _container = container; connect(this, &WimaPlaner::missionReadyChanged, _container, &WimaDataContainer::newDataAvailable); emit dataContainerChanged(); } } 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{ recalcPolygonInteractivity(_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 = ""; emit currentFileChanged(); if ( changesApplied ) emit visualItemsChanged(); } bool WimaPlaner::updateMission() { QString errorString; #define debug 0 if ( !recalcJoinedArea(errorString)) { qgcApp()->showMessage(tr(errorString.toLocal8Bit().data())); return false; } #if debug _visualItems.append(&_joinedArea); #endif // extract old survey data QmlObjectListModel* missionItems = _missionController->visualItems(); CircularSurveyComplexItem* OldSurveyPt = nullptr; QGeoCoordinate oldSurveyRef; bool oldSurveyExists = false; for (int i = 0; i < _missionController->visualItems()->count(); i++) { OldSurveyPt = qobject_cast(missionItems->get(i)); if ( OldSurveyPt != nullptr) { oldSurveyRef = OldSurveyPt->refPoint(); oldSurveyExists = true; break; } } // reset visual items _missionController->removeAll(); 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 = _serviceArea.center(); center.setAltitude(0); _serviceArea.setCenter(center); center = _measurementArea.center(); center.setAltitude(0); _measurementArea.setCenter(center); center = _corridor.center(); center.setAltitude(0); _corridor.setCenter(center); // set HomePos. to serArea center settingsItem->setCoordinate(_serviceArea.center()); // create take off position item int sequenceNumber = _missionController->insertSimpleMissionItem(_serviceArea.center(), missionItems->count()); _missionController->setCurrentPlanViewIndex(sequenceNumber, true); SimpleMissionItem* takeOffItem = qobject_cast(missionItems->get(missionItems->count()-1)); if (takeOffItem == nullptr){ qWarning("WimaPlaner::updateMission(): takeOffItem == nullptr"); return false; } else { takeOffItem->setCoordinate(_serviceArea.center()); //necessary, insertSimpleMissionItem does not copy coordinate (why?) } // create survey item, will be extened with more()-> mission types in the future _missionController->insertComplexMissionItem(_missionController->circularSurveyComplexItemName(), _measurementArea.center(), missionItems->count()); CircularSurveyComplexItem* survey = qobject_cast(missionItems->get(missionItems->count()-1)); if (survey == nullptr){ qWarning("WimaPlaner::updateMission(): survey == nullptr"); return false; } if ( oldSurveyExists ) { survey->setRefPoint(oldSurveyRef); disconnect(OldSurveyPt->deltaR(), &Fact::rawValueChanged, this, &WimaPlaner::resetMissionReady); disconnect(OldSurveyPt->deltaAlpha(), &Fact::rawValueChanged, this, &WimaPlaner::resetMissionReady); disconnect(OldSurveyPt->isSnakePath(), &Fact::rawValueChanged, this, &WimaPlaner::resetMissionReady); disconnect(OldSurveyPt->transectMinLength(), &Fact::rawValueChanged, this, &WimaPlaner::resetMissionReady); } else { survey->setRefPoint(_measurementArea.center()); } survey->setAutoGenerated(true); // prevents reinitialisation from gui survey->surveyAreaPolygon()->clear(); survey->surveyAreaPolygon()->appendVertices(_measurementArea.coordinateList()); connect(survey->deltaR(), &Fact::rawValueChanged, this, &WimaPlaner::resetMissionReady); connect(survey->deltaAlpha(), &Fact::rawValueChanged, this, &WimaPlaner::resetMissionReady); connect(survey->isSnakePath(), &Fact::rawValueChanged, this, &WimaPlaner::resetMissionReady); connect(survey->transectMinLength(), &Fact::rawValueChanged, this, &WimaPlaner::resetMissionReady); // calculate path from take off to opArea if (survey->visualTransectPoints().size() == 0) { qWarning("WimaPlaner::updateMission(): survey no points."); return false; } QGeoCoordinate start = _serviceArea.center(); QGeoCoordinate end = survey->coordinate(); #ifdef QT_DEBUG if (!_visualItems.contains(&_joinedArea)) _visualItems.append(&_joinedArea); #endif QList path; if ( !calcShortestPath(start, end, path)) { qgcApp()->showMessage( QString(tr("Not able to calculate the path from takeoff position to measurement area.")).toLocal8Bit().data()); return false; } _arrivalPathLength = path.size()-1; // -1: last item is first measurement point 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->exitCoordinate(); end = _serviceArea.center(); path.clear(); if ( !calcShortestPath(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.value(i), missionItems->count()); _missionController->setCurrentPlanViewIndex(sequenceNumber, true); } // create land position item sequenceNumber = _missionController->insertSimpleMissionItem(_serviceArea.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 { if (!_missionController->setLandCommand(*landItem, *_masterController->controllerVehicle())) return false; } pushToContainer(); // exchange plan data with the WimaController via the _container setMissionReady(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] == WimaMeasurementArea::WimaMeasurementAreaName) { print(_measurementArea); bool success = _measurementArea.loadFromJson(jsonArea, errorString); print(_measurementArea); 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(); //recalcJoinedArea(); // MissionItems // extrac MissionItems part // bool ret = json.contains(missionItemsName); // qWarning() << ret; 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); // 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->setWimaAreaInteractive(true); } } bool WimaPlaner::recalcJoinedArea(QString &errorString) { // check if area paths form simple polygons if ( !_serviceArea.isSimplePolygon() ) { errorString.append(tr("Service area is self intersecting and thus not a simple polygon. Only simple polygons allowed.\n")); return false; } if ( !_corridor.isSimplePolygon() && _corridor.count() > 0) { errorString.append(tr("Corridor is self intersecting and thus not a simple polygon. Only simple polygons allowed.\n")); return false; } if ( !_measurementArea.isSimplePolygon() ) { errorString.append(tr("Measurement area is self intersecting and thus not a simple polygon. Only simple polygons allowed.\n")); return false; } _joinedArea.setPath(_serviceArea.path()); _joinedArea.join(_corridor); if ( !_joinedArea.join(_measurementArea) ) { errorString.append(tr("Not able to join areas. Service area and measurement" " must have a overlapping section, or be connected through a corridor.")); return false; // this happens if all areas are pairwise disjoint } // join service area, op area and corridor return true; } /*! * \fn void WimaPlaner::pushToContainer() * Pushes the \c WimaPlanData object generated by \c toPlanData() to the \c WimaDataContainer. * Should be called only after \c updateMission() was successful. * * \sa WimaDataContainer, WimaPlanData */ void WimaPlaner::pushToContainer() { if (_container != nullptr) { WimaPlanData planData = toPlanData(); _container->push(planData); } else { qWarning("WimaPlaner::uploadToContainer(): no container assigned."); } } bool WimaPlaner::calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QList &path) { using namespace GeoUtilities; using namespace PolygonCalculus; QList path2D; bool retVal = PolygonCalculus::shortestPath( toQPolygonF(toCartesian2D(_joinedArea.coordinateList(), /*origin*/ start)), /*start point*/ QPointF(0,0), /*destination*/ toCartesian2D(destination, start), /*shortest path*/ path2D); path.append(toGeo(path2D, /*origin*/ start)); return retVal; } 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() { recalcPolygonInteractivity(_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 updateMission() was successful. * * \sa WimaController, WimaPlanData */ WimaPlanData WimaPlaner::toPlanData() { WimaPlanData planData; // 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 rgMissionItems; MissionController::convertToMissionItems(_missionController->visualItems(), rgMissionItems, this); // add const qualifier... QList rgMissionItemsConst; for (int i = _arrivalPathLength + 1; i < rgMissionItems.size() - _returnPathLength; i++) { // i = _arrivalPathLength + 1: + 1 = MissionSettingsItem ... rgMissionItemsConst.append(rgMissionItems.value(i)); } // store mavlink commands planData.append(rgMissionItemsConst); return planData; } void WimaPlaner::setMissionReady(bool ready) { if(_missionReady != ready) { _missionReady = ready; emit missionReadyChanged(_missionReady); } } void WimaPlaner::resetMissionReady() { setMissionReady(false); } 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); }