WimaPlaner.cc 29 KB
Newer Older
1
 #include "WimaPlaner.h"
2

3 4
#include "CircularSurveyComplexItem.h"

5 6


7 8 9 10 11
const char* WimaPlaner::wimaFileExtension   = "wima";
const char* WimaPlaner::areaItemsName       = "AreaItems";
const char* WimaPlaner::missionItemsName    = "MissionItems";

WimaPlaner::WimaPlaner(QObject *parent)
12 13 14 15
    : QObject                       (parent)
    , _currentAreaIndex             (-1)
    , _container                    (nullptr)
    , _joinedArea                   (this)
16
    , _joinedAreaValid              (false)
17 18 19 20 21
    , _measurementArea              (this)
    , _serviceArea                  (this)
    , _corridor                     (this)
    , _circularSurvey               (nullptr)
    , _surveyRefChanging            (false)
22 23 24
    , _measurementAreaChanging      (false)
    , _corridorChanging             (false)
    , _serviceAreaChanging          (false)
25
    , _syncronizedWithController    (false)
26
    , _readyForSync                 (false)
27
{
28 29 30 31
    _lastMeasurementAreaPath    = _measurementArea.path();
    _lastServiceAreaPath        = _serviceArea.path();
    _lastCorridorPath           = _corridor.path();

32
    connect(this, &WimaPlaner::currentPolygonIndexChanged, this, &WimaPlaner::recalcPolygonInteractivity);
33
    connect(&_updateTimer, &QTimer::timeout, this, &WimaPlaner::updateTimerSlot);
34
    connect(this, &WimaPlaner::joinedAreaValidChanged, this, &WimaPlaner::updateMission);
35

36
    _updateTimer.setInterval(500); // 250 ms means: max update time 2*250 ms
37
    _updateTimer.start();
38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89
}

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)
{
90
    if (container != nullptr) {
91 92 93 94 95
        _container = container;
        emit dataContainerChanged();
    }
}

96 97 98 99 100
bool WimaPlaner::syncronizedWithController()
{
    return _syncronizedWithController;
}

101 102 103 104 105
bool WimaPlaner::readyForSync()
{
    return _readyForSync;
}

106 107 108 109 110 111 112 113 114 115
void WimaPlaner::removeArea(int index)
{
    if(index >= 0 && index < _visualItems.count()){
        WimaArea* area = qobject_cast<WimaArea*>(_visualItems.removeAt(index));

        if ( area == nullptr) {
            qWarning("WimaPlaner::removeArea(): nullptr catched, internal error.");
            return;
        }
        area->clear();
116
        area->borderPolygon()->clear();
117 118 119 120 121 122

        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.
123
            resetAllInteractive();
124 125 126 127 128 129 130 131 132 133 134 135 136 137 138
            _currentAreaIndex = -1;
            return;
        }

        if(_currentAreaIndex >= _visualItems.count()){
            setCurrentPolygonIndex(_visualItems.count() - 1);
        }else{
            recalcPolygonInteractivity(_currentAreaIndex);
        }
    }else{
        qWarning("Index out of bounds!");
    }

}

139
bool WimaPlaner::addMeasurementArea()
140
{
141 142
    if (!_visualItems.contains(&_measurementArea)) {
        _visualItems.append(&_measurementArea);
143 144 145 146 147 148 149 150 151 152 153 154 155

        int newIndex = _visualItems.count()-1;
        setCurrentPolygonIndex(newIndex);

        emit visualItemsChanged();
        return true;
    } else {
        return false;
    }
}

bool WimaPlaner::addServiceArea()
{
156 157
    if (!_visualItems.contains(&_serviceArea)) {
        _visualItems.append(&_serviceArea);
158 159 160 161 162 163 164 165 166 167 168

        int newIndex = _visualItems.count()-1;
        setCurrentPolygonIndex(newIndex);

        emit visualItemsChanged();
        return true;
    } else {
        return false;
    }
}

169
bool WimaPlaner::addCorridor()
170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195
{
    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 = "";

196 197 198 199
    _circularSurvey = nullptr;
    _surveyRefChanging = false;


200 201 202 203 204 205 206
    emit currentFileChanged();
    if ( changesApplied )
         emit visualItemsChanged();
}

bool WimaPlaner::updateMission()
{
207

208
   setReadyForSync(false);
209

210
    if ( !_joinedAreaValid)
211 212 213
        return false;


Valentin Platzgummer's avatar
Valentin Platzgummer committed
214 215 216
    // extract old survey data
    QmlObjectListModel* missionItems        = _missionController->visualItems();

217
    int surveyIndex = missionItems->indexOf(_circularSurvey);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
218

219
    // create survey item if not yet present
220
    if (surveyIndex == -1) {
221
        _missionController->insertComplexMissionItem(_missionController->circularSurveyComplexItemName(), _measurementArea.center(), missionItems->count());
222
        _circularSurvey = qobject_cast<CircularSurveyComplexItem*>(missionItems->get(missionItems->count()-1));
223

224
        if (_circularSurvey == nullptr){
225 226 227 228
            qWarning("WimaPlaner::updateMission(): survey == nullptr");
            return false;
        }

229

230 231
        // establish connections
        _circularSurvey->setRefPoint(_measurementArea.center());
232 233
        _lastSurveyRefPoint = _measurementArea.center();
        _surveyRefChanging = false;
234
        _circularSurvey->setIsInitialized(true); // prevents reinitialisation from gui
235 236 237 238
        connect(_circularSurvey->deltaR(),               &Fact::rawValueChanged, this, &WimaPlaner::calcArrivalAndReturnPath);
        connect(_circularSurvey->deltaAlpha(),           &Fact::rawValueChanged, this, &WimaPlaner::calcArrivalAndReturnPath);
        connect(_circularSurvey->isSnakePath(),          &Fact::rawValueChanged, this, &WimaPlaner::calcArrivalAndReturnPath);
        connect(_circularSurvey->transectMinLength(),    &Fact::rawValueChanged, this, &WimaPlaner::calcArrivalAndReturnPath);
239 240
    }

241 242 243
    // update survey area
    _circularSurvey->surveyAreaPolygon()->clear();
    _circularSurvey->surveyAreaPolygon()->appendVertices(_measurementArea.coordinateList());
244

245
    calcArrivalAndReturnPath();
246

247
    setReadyForSync(true);
248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338
    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()) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
339
                if ( jsonArea[WimaArea::areaTypeName] == WimaMeasurementArea::WimaMeasurementAreaName) {
340
                    bool success = _measurementArea.loadFromJson(jsonArea, errorString);
341 342 343 344 345 346

                    if ( !success ) {
                        qgcApp()->showMessage(errorMessage.arg(errorString));
                        return false;
                    }

347
                    _lastMeasurementAreaPath    = _measurementArea.path(); // prevents error messages at this point
348
                    validAreaCounter++;
349
                    _visualItems.append(&_measurementArea);
350 351
                    emit visualItemsChanged();
                } else if ( jsonArea[WimaArea::areaTypeName] == WimaServiceArea::wimaServiceAreaName) {
352
                    bool success = _serviceArea.loadFromJson(jsonArea, errorString);
353 354 355 356 357 358

                    if ( !success ) {
                        qgcApp()->showMessage(errorMessage.arg(errorString));
                        return false;
                    }

359
                    _lastServiceAreaPath        = _serviceArea.path(); // prevents error messages at this point
360
                    validAreaCounter++;
361
                    _visualItems.append(&_serviceArea);
362
                    emit visualItemsChanged();
Valentin Platzgummer's avatar
Valentin Platzgummer committed
363
                } else if ( jsonArea[WimaArea::areaTypeName] == WimaCorridor::WimaCorridorName) {
364 365 366 367 368 369 370
                    bool success = _corridor.loadFromJson(jsonArea, errorString);

                    if ( !success ) {
                        qgcApp()->showMessage(errorMessage.arg(errorString));
                        return false;
                    }

371
                    _lastCorridorPath           = _corridor.path(); // prevents error messages at this point
372 373 374 375 376 377 378 379 380 381 382 383
                    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;
            }
384 385
        }        
        recalcJoinedArea();
386 387 388 389

        _currentFile.sprintf("%s/%s.%s", fileInfo.path().toLocal8Bit().data(), fileInfo.completeBaseName().toLocal8Bit().data(), wimaFileExtension);

        emit currentFileChanged();
Valentin Platzgummer's avatar
Valentin Platzgummer committed
390 391 392 393 394 395

        QJsonObject missionObject = json[missionItemsName].toObject();

        //qWarning() << json[missionItemsName].type();

        QJsonDocument missionJsonDoc = QJsonDocument(missionObject);
396 397 398 399 400 401 402 403
        // 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++) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
404 405
            temporaryFileName = cropedFileName + QString("/temp%1.%2").arg(i).arg(AppSettings::planFileExtension);
            // qWarning() << temporaryFileName;
406 407 408 409 410 411 412 413 414 415 416 417 418 419

            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;
            }
        }

Valentin Platzgummer's avatar
Valentin Platzgummer committed
420
        // qWarning() << missionJsonDoc.toVariant().toString();
421
        temporaryFile.write(missionJsonDoc.toJson());
Valentin Platzgummer's avatar
Valentin Platzgummer committed
422
        temporaryFile.close();
423 424 425 426

        // load from temporary file
        _masterController->loadFromFile(temporaryFileName);

427 428 429 430 431 432 433 434 435
        QmlObjectListModel *missionItems = _missionController->visualItems();

        _circularSurvey = nullptr;
        for (int i = 0; i < missionItems->count(); i++) {
            _circularSurvey = missionItems->value<CircularSurveyComplexItem *>(i);
            if (_circularSurvey != nullptr) {

                _lastSurveyRefPoint = _circularSurvey->refPoint();
                _surveyRefChanging = false;
436
                _circularSurvey->setIsInitialized(true); // prevents reinitialisation from gui
437 438 439 440 441 442 443 444
                connect(_circularSurvey->deltaR(),               &Fact::rawValueChanged, this, &WimaPlaner::calcArrivalAndReturnPath);
                connect(_circularSurvey->deltaAlpha(),           &Fact::rawValueChanged, this, &WimaPlaner::calcArrivalAndReturnPath);
                connect(_circularSurvey->isSnakePath(),          &Fact::rawValueChanged, this, &WimaPlaner::calcArrivalAndReturnPath);
                connect(_circularSurvey->transectMinLength(),    &Fact::rawValueChanged, this, &WimaPlaner::calcArrivalAndReturnPath);
                break;
            }
        }

445 446
        //if (_circularSurvey == nullptr)
        updateMission();
447

448 449 450 451 452
        // remove temporary file
        if ( !temporaryFile.remove() ){
            qWarning("WimaPlaner::loadFromFile(): not able to remove temporary file.");
        }

453
        setSyncronizedWithController(false);
454

455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472
        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<WimaArea*>(_visualItems.get(index));
473
        interactivePoly->setWimaAreaInteractive(true);
474 475 476
    }
}

477 478
bool WimaPlaner::calcArrivalAndReturnPath()
{
479 480
    setReadyForSync(false);

481 482 483 484 485 486 487 488 489 490
    // extract old survey data
    QmlObjectListModel          *missionItems   = _missionController->visualItems();

    int surveyIndex = missionItems->indexOf(_circularSurvey);

    if (surveyIndex == -1) {
        qWarning("WimaPlaner::calcArrivalAndReturnPath(): no survey item");
        return false;
    }

491 492 493 494
    bool restorePlanViewIndex = false;
    if (surveyIndex == _missionController->currentPlanViewIndex())
        restorePlanViewIndex = true;

495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546
    // 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<MissionSettingsItem*>(missionItems->get(0));
    if (settingsItem == nullptr){
        qWarning("WimaPlaner::calcArrivalAndReturnPath(): 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());

    // set takeoff position
    bool setCommandNeeded = false;
    if (missionItems->count() < 3) {
        setCommandNeeded = true;
        _missionController->insertSimpleMissionItem(_serviceArea.center(), 1);
    }
    SimpleMissionItem* takeOffItem = qobject_cast<SimpleMissionItem*>(missionItems->get(1));
    if (takeOffItem == nullptr){
        qWarning("WimaPlaner::calcArrivalAndReturnPath(): takeOffItem == nullptr");
        return false;
    }
    if (setCommandNeeded)
        _missionController->setTakeoffCommand(*takeOffItem);
    takeOffItem->setCoordinate(_serviceArea.center());

    if (_circularSurvey->visualTransectPoints().size() == 0) {
        qWarning("WimaPlaner::calcArrivalAndReturnPath(): survey no points.");
        return false;
    }

    // calculate path from take off to survey
    QGeoCoordinate start = _serviceArea.center();
    QGeoCoordinate end = _circularSurvey->coordinate();

    #ifdef QT_DEBUG
547 548
    //if (!_visualItems.contains(&_joinedArea))
        //_visualItems.append(&_joinedArea);
549 550 551 552 553 554 555
    #endif

    QList<QGeoCoordinate> 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;
    }
556
    _arrivalPathLength = path.size()-1;
557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588
    int sequenceNumber = 0;
    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   = _circularSurvey->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<SimpleMissionItem*>(missionItems->get(missionItems->count()-1));
    if (landItem == nullptr){
        qWarning("WimaPlaner::calcArrivalAndReturnPath(): landItem == nullptr");
        return false;
    } else {
        if (!_missionController->setLandCommand(*landItem))
            return false;
    }

589 590 591
    if (restorePlanViewIndex)
        _missionController->setCurrentPlanViewIndex(missionItems->indexOf(_circularSurvey), false);
    setSyncronizedWithControllerFalse();
592
    setReadyForSync(true);
593 594 595
    return true;
}

596
bool WimaPlaner::recalcJoinedArea()
597
{
598
    setJoinedAreaValid(false);
599 600 601 602
    // check if at least service area and measurement area are available
    if (_visualItems.indexOf(&_serviceArea) == -1 || _visualItems.indexOf(&_measurementArea) == -1)
        return false;

603
    // check if area paths form simple polygons
604
    if ( !_serviceArea.isSimplePolygon() ) {
605
        qgcApp()->showMessage(tr("Service area is self intersecting and thus not a simple polygon. Only simple polygons allowed.\n"));
606 607 608
        return false;
    }

609
    if ( !_corridor.isSimplePolygon() && _corridor.count() > 0) {
610
        qgcApp()->showMessage(tr("Corridor is self intersecting and thus not a simple polygon. Only simple polygons allowed.\n"));
611 612 613
        return false;
    }

614
    if ( !_measurementArea.isSimplePolygon() ) {
615
        qgcApp()->showMessage(tr("Measurement area is self intersecting and thus not a simple polygon. Only simple polygons allowed.\n"));
616 617 618
        return false;
    }

619
    _joinedArea.setPath(_serviceArea.path());
620
    _joinedArea.join(_corridor);
621
    if ( !_joinedArea.join(_measurementArea) ) {
622 623
        qgcApp()->showMessage(tr("Not able to join areas. Service area and measurement"
                                 " must have a overlapping section, or be connected through a corridor."));
624
        return false; // this happens if all areas are pairwise disjoint
625
    }
626

Valentin Platzgummer's avatar
Valentin Platzgummer committed
627
    // join service area, op area and corridor
628
    setJoinedAreaValid(true);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
629
    return true;
630 631
}

632 633 634 635 636 637 638
/*!
 * \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
 */
639 640 641
void WimaPlaner::pushToContainer()
{
    if (_container != nullptr) {
642 643
        if (!_readyForSync)
            return;
644 645
        WimaPlanData planData = toPlanData();
        _container->push(planData);
646
        setSyncronizedWithController(true);
647 648 649 650 651
    } else {
        qWarning("WimaPlaner::uploadToContainer(): no container assigned.");
    }
}

652 653 654 655 656 657
bool WimaPlaner::calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QList<QGeoCoordinate> &path)
{
    using namespace GeoUtilities;
    using namespace PolygonCalculus;
    QList<QPointF> path2D;
    bool retVal = PolygonCalculus::shortestPath(
Valentin Platzgummer's avatar
Valentin Platzgummer committed
658
                                   toQPolygonF(toCartesian2D(_joinedArea.coordinateList(), /*origin*/ start)),
659
                                   /*start point*/ QPointF(0,0),
Valentin Platzgummer's avatar
Valentin Platzgummer committed
660 661
                                   /*destination*/ toCartesian2D(destination, start),
                                   /*shortest path*/ path2D);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
662
    path.append(toGeo(path2D, /*origin*/ start));
663 664 665 666

    return  retVal;
}

667
void WimaPlaner::resetAllInteractive()
668
{
669 670 671 672 673
    // 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<WimaArea*>(_visualItems.get(i));
674
            iteratorPoly->setWimaAreaInteractive(false);
675 676 677 678 679 680 681 682 683
        }
    }
}

void WimaPlaner::setInteractive()
{
    recalcPolygonInteractivity(_currentAreaIndex);
}

684 685 686 687 688 689 690 691 692 693 694 695
/*!
 * \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;
696 697

    // store areas
698 699 700
    planData.append(WimaMeasurementAreaData(_measurementArea));
    planData.append(WimaServiceAreaData(_serviceArea));
    planData.append(WimaCorridorData(_corridor));
701
    planData.append(WimaJoinedAreaData(_joinedArea));
702

703
    // convert mission items to mavlink commands
704
    QObject deleteObject; // used to automatically delete content of rgMissionItems after this function call
705
    QList<MissionItem*> rgMissionItems;
706
    MissionController::convertToMissionItems(_missionController->visualItems(), rgMissionItems, &deleteObject);
707

708
    // store mavlink commands
709
    planData.append(rgMissionItems);
710 711

    return planData;
712 713
}

714
void WimaPlaner::setSyncronizedWithController(bool sync)
715
{
716 717 718
    if (_syncronizedWithController != sync) {
        _syncronizedWithController = sync;
        emit syncronizedWithControllerChanged();
719 720 721
    }
}

722 723 724 725 726 727 728 729 730 731 732 733 734 735 736 737 738 739
void WimaPlaner::setReadyForSync(bool ready)
{
    if( _readyForSync != ready) {
        _readyForSync = ready;

        emit readyForSyncChanged();
    }
}

void WimaPlaner::setJoinedAreaValid(bool valid)
{
    if (_joinedAreaValid != valid) {
        _joinedAreaValid = valid;

        emit joinedAreaValidChanged();
    }
}

740 741 742 743 744 745
void WimaPlaner::updateTimerSlot()
{
    // General operation of this function:
    // Check if parameter has changed, wait until it stops changing, update mission

    // circular survey reference point
746 747 748 749
    if (_missionController != nullptr
            && _missionController->visualItems()->indexOf(_circularSurvey) != -1
            && _circularSurvey != nullptr)
    {
750 751 752 753 754 755 756 757 758 759 760
        if (_surveyRefChanging) {
            if (_circularSurvey->refPoint() == _lastSurveyRefPoint) { // is it still changing?
                calcArrivalAndReturnPath();
                _surveyRefChanging = false;
            }
        } else {
            if (_circularSurvey->refPoint() != _lastSurveyRefPoint) // does it started changing?
                _surveyRefChanging = true;
        }
    }

761 762 763 764 765 766 767 768 769 770 771 772 773 774 775 776 777 778 779 780 781 782 783 784 785 786 787 788 789 790 791 792 793
    // measurementArea
    if (_measurementAreaChanging) {
        if (_measurementArea.path() == _lastMeasurementAreaPath) { // is it still changing?
            recalcJoinedArea();
            _measurementAreaChanging = false;
        }
    } else {
        if (_measurementArea.path() != _lastMeasurementAreaPath) // does it started changing?
            _measurementAreaChanging = true;
    }

    // corridor
    if (_corridorChanging) {
        if (_corridor.path() == _lastCorridorPath) { // is it still changing?
            recalcJoinedArea();
            _corridorChanging = false;
        }
    } else {
        if (_corridor.path() != _lastCorridorPath) // does it started changing?
            _corridorChanging = true;
    }

    // service area
    if (_serviceAreaChanging) {
        if (_serviceArea.path() == _lastServiceAreaPath) { // is it still changing?
            recalcJoinedArea();
            _serviceAreaChanging = false;
        }
    } else {
        if (_serviceArea.path() != _lastServiceAreaPath) // does it started changing?
            _serviceAreaChanging = true;
    }

794 795

    // update old values
796 797 798
    if (_missionController != nullptr
            && _missionController->visualItems()->indexOf(_circularSurvey) != -1
            && _circularSurvey != nullptr)
799 800 801 802 803
        _lastSurveyRefPoint = _circularSurvey->refPoint();

    _lastMeasurementAreaPath    = _measurementArea.path();
    _lastCorridorPath           = _corridor.path();
    _lastServiceAreaPath        = _serviceArea.path();
804 805
}

806 807 808 809 810
void WimaPlaner::setSyncronizedWithControllerFalse()
{
    setSyncronizedWithController(false);
}

811 812 813 814 815 816 817 818 819 820 821 822 823 824 825 826 827 828 829 830
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<WimaArea*>(_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
Valentin Platzgummer's avatar
Valentin Platzgummer committed
831
            WimaMeasurementArea* opArea =  qobject_cast<WimaMeasurementArea*>(area);
832 833 834 835 836
            if (opArea != nullptr) {
                opArea->saveToJson(json);
                jsonArray.append(json);
                continue;
            }
837

838 839 840 841 842 843 844
            WimaServiceArea* serArea =  qobject_cast<WimaServiceArea*>(area);
            if (serArea != nullptr) {
                serArea->saveToJson(json);
                jsonArray.append(json);
                continue;
            }

Valentin Platzgummer's avatar
Valentin Platzgummer committed
845
            WimaCorridor* corridor =  qobject_cast<WimaCorridor*>(area);
846 847 848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863 864 865
            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);
866
}
867 868 869