WimaController.cc 30.1 KB
Newer Older
Valentin Platzgummer's avatar
Valentin Platzgummer committed
1
#include "WimaController.h"
Valentin Platzgummer's avatar
Valentin Platzgummer committed
2
#include "utilities.h"
3
#include "ros_bridge/include/JsonMethodes.h"
4 5 6
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h"
Valentin Platzgummer's avatar
Valentin Platzgummer committed
7
#include "ros_bridge/include/CasePacker.h"
Valentin Platzgummer's avatar
Valentin Platzgummer committed
8

9 10 11
#include "Snake/QtROSJsonFactory.h"
#include "Snake/QtROSTypeFactory.h"
#include "Snake/QNemoProgress.h"
12
#include "Snake/QNemoHeartbeat.h"
13

14
#include "QVector3D"
15
#include <QScopedPointer>
16

17

18

19
// const char* WimaController::wimaFileExtension           = "wima";
20 21 22 23 24 25 26 27 28
const char* WimaController::areaItemsName               = "AreaItems";
const char* WimaController::missionItemsName            = "MissionItems";
const char* WimaController::settingsGroup               = "WimaController";
const char* WimaController::enableWimaControllerName    = "EnableWimaController";
const char* WimaController::overlapWaypointsName        = "OverlapWaypoints";
const char* WimaController::maxWaypointsPerPhaseName    = "MaxWaypointsPerPhase";
const char* WimaController::startWaypointIndexName      = "StartWaypointIndex";
const char* WimaController::showAllMissionItemsName     = "ShowAllMissionItems";
const char* WimaController::showCurrentMissionItemsName = "ShowCurrentMissionItems";
29
const char* WimaController::flightSpeedName             = "FlightSpeed";
30
const char* WimaController::arrivalReturnSpeedName      = "ArrivalReturnSpeed";
31
const char* WimaController::altitudeName                = "Altitude";
Valentin Platzgummer's avatar
Valentin Platzgummer committed
32 33
const char* WimaController::snakeTileWidthName          = "SnakeTileWidth";
const char* WimaController::snakeTileHeightName         = "SnakeTileHeight";
34
const char* WimaController::snakeMinTileAreaName        = "SnakeMinTileArea";
Valentin Platzgummer's avatar
Valentin Platzgummer committed
35 36 37
const char* WimaController::snakeLineDistanceName       = "SnakeLineDistance";
const char* WimaController::snakeMinTransectLengthName  = "SnakeMinTransectLength";

Valentin Platzgummer's avatar
Valentin Platzgummer committed
38 39 40 41
WimaController::StatusMap WimaController::_nemoStatusMap{
    std::make_pair<int, QString>(0, "No Heartbeat"),
    std::make_pair<int, QString>(1, "Connected"),
    std::make_pair<int, QString>(-1, "Timeout")};
42

Valentin Platzgummer's avatar
Valentin Platzgummer committed
43 44
using namespace snake;
using namespace snake_geometry;
45

46
WimaController::WimaController(QObject *parent)
47
    : QObject                   (parent)
48 49 50 51
    , _joinedArea               ()
    , _measurementArea          ()
    , _serviceArea              ()
    , _corridor                 ()
52
    , _localPlanDataValid       (false)
Valentin Platzgummer's avatar
Valentin Platzgummer committed
53 54 55 56
    , _areaInterface            (&_measurementArea, &_serviceArea, &_corridor, &_joinedArea)
    , _managerSettings          ()
    , _defaultManager           (_managerSettings, _areaInterface)
    , _snakeManager             (_managerSettings, _areaInterface)
57 58
    , _rtlManager               (_managerSettings, _areaInterface)
    , _currentManager           (&_defaultManager)
Valentin Platzgummer's avatar
Valentin Platzgummer committed
59
    , _managerList              {&_defaultManager, &_snakeManager, &_rtlManager}
Valentin Platzgummer's avatar
Valentin Platzgummer committed
60 61
    , _metaDataMap              (FactMetaData::createMapFromJsonFile(
                                     QStringLiteral(":/json/WimaController.SettingsGroup.json"), this))
62 63 64
    , _enableWimaController     (settingsGroup, _metaDataMap[enableWimaControllerName])
    , _overlapWaypoints         (settingsGroup, _metaDataMap[overlapWaypointsName])
    , _maxWaypointsPerPhase     (settingsGroup, _metaDataMap[maxWaypointsPerPhaseName])
65
    , _nextPhaseStartWaypointIndex       (settingsGroup, _metaDataMap[startWaypointIndexName])
66
    , _showAllMissionItems      (settingsGroup, _metaDataMap[showAllMissionItemsName])
67 68
    , _showCurrentMissionItems  (settingsGroup, _metaDataMap[showCurrentMissionItemsName])    
    , _flightSpeed              (settingsGroup, _metaDataMap[flightSpeedName])
69
    , _arrivalReturnSpeed       (settingsGroup, _metaDataMap[arrivalReturnSpeedName])
70
    , _altitude                 (settingsGroup, _metaDataMap[altitudeName])
71 72
    , _measurementPathLength    (-1)
    , _lowBatteryHandlingTriggered  (false)
Valentin Platzgummer's avatar
Valentin Platzgummer committed
73 74 75 76 77 78
    , _snakeCalcInProgress      (false)
    , _snakeTileWidth           (settingsGroup, _metaDataMap[snakeTileWidthName])
    , _snakeTileHeight          (settingsGroup, _metaDataMap[snakeTileHeightName])
    , _snakeMinTileArea         (settingsGroup, _metaDataMap[snakeMinTileAreaName])
    , _snakeLineDistance        (settingsGroup, _metaDataMap[snakeLineDistanceName])
    , _snakeMinTransectLength   (settingsGroup, _metaDataMap[snakeMinTransectLengthName])
79
    , _nemoHeartbeat            (0 /*status: not connected*/)
80 81
    , _fallbackStatus           (0 /*status: not connected*/)
    , _bridgeSetupDone          (false)
Valentin Platzgummer's avatar
Valentin Platzgummer committed
82
    , _pRosBridge               (new ROSBridge::ROSBridge())
83
{
84
    // Set up facts.
85 86
    _showAllMissionItems.setRawValue(true);
    _showCurrentMissionItems.setRawValue(true);
87 88
    connect(&_overlapWaypoints,             &Fact::rawValueChanged, this, &WimaController::_updateOverlap);
    connect(&_maxWaypointsPerPhase,         &Fact::rawValueChanged, this, &WimaController::_updateMaxWaypoints);
89
    connect(&_nextPhaseStartWaypointIndex,  &Fact::rawValueChanged, this, &WimaController::_setStartIndex);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
90 91 92
    connect(&_flightSpeed,                  &Fact::rawValueChanged, this, &WimaController::_updateflightSpeed);
    connect(&_arrivalReturnSpeed,           &Fact::rawValueChanged, this, &WimaController::_updateArrivalReturnSpeed);
    connect(&_altitude,                     &Fact::rawValueChanged, this, &WimaController::_updateAltitude);
93

Valentin Platzgummer's avatar
Valentin Platzgummer committed
94 95 96 97 98 99 100 101 102 103 104 105 106 107
    // Init waypoint managers.
    bool value;
    size_t overlap  = _overlapWaypoints.rawValue().toUInt(&value);
    Q_ASSERT(value);
    size_t N        = _maxWaypointsPerPhase.rawValue().toUInt(&value);
    Q_ASSERT(value);
    size_t startIdx = _nextPhaseStartWaypointIndex.rawValue().toUInt(&value);
    Q_ASSERT(value);
    (void)value;
    for (auto manager : _managerList){
        manager->setOverlap(overlap);
        manager->setN(N);
        manager->setStartIndex(startIdx);
    }
108

109
    // Periodic.
Valentin Platzgummer's avatar
Valentin Platzgummer committed
110
    connect(&_eventTimer, &QTimer::timeout, this, &WimaController::_eventTimerHandler);
111 112
    //_eventTimer.setInterval(EVENT_TIMER_INTERVAL);
    _eventTimer.start(EVENT_TIMER_INTERVAL);
113

Valentin Platzgummer's avatar
Valentin Platzgummer committed
114
    // Snake Worker Thread.
115 116 117
    connect(&_snakeWorker, &SnakeWorker::finished, this, &WimaController::_snakeStoreWorkerResults);
    connect(this, &WimaController::nemoProgressChanged, this, &WimaController::_initStartSnakeWorker);
    connect(this, &QObject::destroyed, &this->_snakeWorker, &SnakeWorker::quit);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
118

119
    // Snake.
120 121
    connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_switchSnakeManager);
    _switchSnakeManager(_enableSnake.rawValue());
122 123
}

124 125 126 127 128 129 130 131 132
PlanMasterController *WimaController::masterController() {
    return _masterController;
}

MissionController *WimaController::missionController() {
    return _missionController;
}

QmlObjectListModel *WimaController::visualItems() {
133 134 135 136
    return &_areas;
}

QmlObjectListModel *WimaController::missionItems() {
137
    return const_cast<QmlObjectListModel*>(&_currentManager->missionItems());
138 139 140
}

QmlObjectListModel *WimaController::currentMissionItems() {
141
    return const_cast<QmlObjectListModel*>(&_currentManager->currentMissionItems());
142 143 144 145
}

QVariantList WimaController::waypointPath()
{
146
    return const_cast<QVariantList&>(_currentManager->waypointsVariant());
147 148
}

149
QVariantList WimaController::currentWaypointPath()
150
{
151
    return const_cast<QVariantList&>(_currentManager->currentWaypointsVariant());
152 153
}

154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189
Fact *WimaController::enableWimaController() {
    return &_enableWimaController;
}

Fact *WimaController::overlapWaypoints() {
    return &_overlapWaypoints;
}

Fact *WimaController::maxWaypointsPerPhase() {
    return &_maxWaypointsPerPhase;
}

Fact *WimaController::startWaypointIndex() {
    return &_nextPhaseStartWaypointIndex;
}

Fact *WimaController::showAllMissionItems() {
    return &_showAllMissionItems;
}

Fact *WimaController::showCurrentMissionItems() {
    return &_showCurrentMissionItems;
}

Fact *WimaController::flightSpeed() {
    return &_flightSpeed;
}

Fact *WimaController::arrivalReturnSpeed() {
    return &_arrivalReturnSpeed;
}

Fact *WimaController::altitude() {
    return &_altitude;
}

190 191 192 193 194 195 196
QVector<int> WimaController::nemoProgress() {
    if ( _nemoProgress.progress().size() == _snakeTileCenterPoints.size() )
        return _nemoProgress.progress();
    else
        return QVector<int>(_snakeTileCenterPoints.size(), 0);
}

197 198
double WimaController::phaseDistance() const
{
199
    return 0.0;
200 201 202 203
}

double WimaController::phaseDuration() const
{
204
    return 0.0;
205 206
}

207 208 209 210 211 212
int WimaController::nemoStatus() const
{
    return _nemoHeartbeat.status();
}

QString WimaController::nemoStatusString() const
Valentin Platzgummer's avatar
Valentin Platzgummer committed
213
{
214
    return _nemoStatusMap.at(_nemoHeartbeat.status());
Valentin Platzgummer's avatar
Valentin Platzgummer committed
215 216 217 218 219 220 221
}

bool WimaController::snakeCalcInProgress() const
{
    return _snakeCalcInProgress;
}

222 223 224
void WimaController::setMasterController(PlanMasterController *masterC)
{
    _masterController = masterC;
225
    _managerSettings.setMasterController(masterC);
226 227 228 229 230 231
    emit masterControllerChanged();
}

void WimaController::setMissionController(MissionController *missionC)
{
    _missionController = missionC;
232
    _managerSettings.setMissionController(missionC);
233 234 235
    emit missionControllerChanged();
}

236 237
void WimaController::nextPhase()
{
Valentin Platzgummer's avatar
Valentin Platzgummer committed
238
    _calcNextPhase();
239 240
}

241
void WimaController::previousPhase()
242
{        
243 244
    if ( !_currentManager->previous() ) {
        Q_ASSERT(false);
245
    }
246 247 248 249 250

    emit missionItemsChanged();
    emit currentMissionItemsChanged();
    emit currentWaypointPathChanged();
    emit waypointPathChanged();
251 252 253 254
}

void WimaController::resetPhase()
{
255 256
    if ( !_currentManager->reset() ) {
        Q_ASSERT(false);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
257
    }
258 259 260 261 262

    emit missionItemsChanged();
    emit currentMissionItemsChanged();
    emit currentWaypointPathChanged();
    emit waypointPathChanged();
263 264
}

265 266
void WimaController::requestSmartRTL()
{
267 268 269 270 271
    QString errorString("Smart RTL requested. ");
    if ( !_checkSmartRTLPreCondition(errorString) ){
        qgcApp()->showMessage(errorString);
        return;
    }
272 273 274 275
    emit smartRTLRequestConfirm();
}

bool WimaController::upload()
276
{
277
    auto &currentMissionItems = _defaultManager.currentMissionItems();
278
    if (   !_serviceArea.containsCoordinate(_masterController->managerVehicle()->coordinate())
279
        && currentMissionItems.count() > 0) {
280
        emit forceUploadConfirm();
281 282 283
        return false;
    }

284
    return forceUpload();
285 286
}

287
bool WimaController::forceUpload()
288
{
289 290
    auto &currentMissionItems = _defaultManager.currentMissionItems();
    if (currentMissionItems.count() < 1)
291
        return false;
292 293

    _missionController->removeAll();
294
    // Set homeposition of settingsItem.
295 296 297
    QmlObjectListModel* visuals = _missionController->visualItems();
    MissionSettingsItem* settingsItem = visuals->value<MissionSettingsItem *>(0);
    if (settingsItem == nullptr) {
298
        Q_ASSERT(false);
299
        qWarning("WimaController::updateCurrentMissionItems(): nullptr");
300
        return false;
301
    }
302
    settingsItem->setCoordinate(_managerSettings.homePosition());
303

304
    // Copy mission items to _missionController.
305
    for (int i = 1; i < currentMissionItems.count(); i++){
306
        auto *item = currentMissionItems.value<const SimpleMissionItem *>(i);
307 308 309
        _missionController->insertSimpleMissionItem(*item, visuals->count());
    }

310
    _masterController->sendToVehicle();
311 312

    return true;
313
}
314

315 316 317
void WimaController::removeFromVehicle()
{
    _masterController->removeAllFromVehicle();
318 319 320
    _missionController->removeAll();
}

321
void WimaController::executeSmartRTL()
322
{
323 324
    forceUpload();
    masterController()->managerVehicle()->startMission();
325 326
}

327
void WimaController::initSmartRTL()
328
{
329
    _initSmartRTL();
330 331
}

332 333 334 335 336 337
void WimaController::removeVehicleTrajectoryHistory()
{
    Vehicle *managerVehicle = masterController()->managerVehicle();
    managerVehicle->trajectoryPoints()->clear();
}

Valentin Platzgummer's avatar
Valentin Platzgummer committed
338 339 340
bool WimaController::_calcShortestPath(const QGeoCoordinate &start,
                                       const QGeoCoordinate &destination,
                                       QVector<QGeoCoordinate> &path)
341 342 343
{
    using namespace GeoUtilities;
    using namespace PolygonCalculus;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
344 345 346 347 348
    QPolygonF polygon2D;
    toCartesianList(_joinedArea.coordinateList(), /*origin*/ start, polygon2D);
    QPointF start2D(0,0);
    QPointF end2D;
    toCartesian(destination, start, end2D);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
349
    QVector<QPointF> path2D;
Valentin Platzgummer's avatar
Valentin Platzgummer committed
350 351 352

    bool retVal = PolygonCalculus::shortestPath(polygon2D, start2D, end2D, path2D);
    toGeoList(path2D, /*origin*/ start, path);
353 354 355 356

    return  retVal;
}

357
bool WimaController::setWimaPlanData(const WimaPlanData &planData)
358
{
359
    // reset visual items
360 361
    _areas.clear();
    _defaultManager.clear();
362 363
    _snakeTiles.polygons().clear();
    _snakeTilesLocal.polygons().clear();
364
    _snakeTileCenterPoints.clear();
365

366 367 368
    emit visualItemsChanged();
    emit missionItemsChanged();
    emit currentMissionItemsChanged();
369
    emit waypointPathChanged();
370
    emit currentWaypointPathChanged();
371
    emit snakeTilesChanged();
372
    emit snakeTileCenterPointsChanged();
373

374
    _localPlanDataValid = false;
375

376 377
    // extract list with WimaAreas
    QList<const WimaAreaData*> areaList = planData.areaList();
378

379
    int areaCounter = 0;
380
    const int numAreas = 4; // extract only numAreas Areas, if there are more they are invalid and ignored
381 382
    for (int i = 0; i < areaList.size(); i++) {
        const WimaAreaData *areaData = areaList[i];
383

384 385 386
        if (areaData->type() == WimaServiceAreaData::typeString) { // is it a service area?
            _serviceArea = *qobject_cast<const WimaServiceAreaData*>(areaData);
            areaCounter++;
387
            _areas.append(&_serviceArea);
388

389 390
            continue;
        }
391

392 393 394
        if (areaData->type() == WimaMeasurementAreaData::typeString) { // is it a measurement area?
            _measurementArea =  *qobject_cast<const WimaMeasurementAreaData*>(areaData);
            areaCounter++;
395
            _areas.append(&_measurementArea);
396

397
            continue;
398
        }
399

400 401 402 403
        if (areaData->type() == WimaCorridorData::typeString) { // is it a corridor?
            _corridor =  *qobject_cast<const WimaCorridorData*>(areaData);
            areaCounter++;
            //_visualItems.append(&_corridor); // not needed
404

405 406
            continue;
        }
407

408 409 410
        if (areaData->type() == WimaJoinedAreaData::typeString) { // is it a corridor?
            _joinedArea =  *qobject_cast<const WimaJoinedAreaData*>(areaData);
            areaCounter++;
411
            _areas.append(&_joinedArea);
412

413 414
            continue;
        }
415

416 417 418
        if (areaCounter >= numAreas)
            break;
    }
419

420
    if (areaCounter != numAreas) {
421
        Q_ASSERT(false);
422 423
        return false;
    }
424

425 426
    emit visualItemsChanged();

427 428 429
    // extract mission items
    QList<MissionItem> tempMissionItems = planData.missionItems();
    if (tempMissionItems.size() < 1) {
430
        qWarning("WimaController: Mission items from WimaPlaner empty!");
431
        return false;
432
    }
433

434
    for (auto item : tempMissionItems) {
435
        _defaultManager.push_back(item.coordinate());
436
    }
437

438 439 440
    _managerSettings.setHomePosition( QGeoCoordinate(_serviceArea.center().latitude(),
                                                     _serviceArea.center().longitude(),
                                                     0) );
441

442
    if( !_defaultManager.reset() ){
443
        Q_ASSERT(false);
444
        return false;
445
    }
446

447 448 449 450 451
    emit missionItemsChanged();
    emit currentMissionItemsChanged();
    emit waypointPathChanged();
    emit currentWaypointPathChanged();

Valentin Platzgummer's avatar
Valentin Platzgummer committed
452
    _localPlanDataValid = true;
453

Valentin Platzgummer's avatar
Valentin Platzgummer committed
454
    _initStartSnakeWorker();
455 456 457

    return true;
}
Valentin Platzgummer's avatar
Valentin Platzgummer committed
458

459 460 461 462 463 464 465 466 467 468 469
WimaController *WimaController::thisPointer()
{
    return this;
}

bool WimaController::_calcNextPhase()
{
    if ( !_currentManager->next() ) {
        Q_ASSERT(false);
        return false;
    }
470

471
    emit missionItemsChanged();
472 473
    emit currentMissionItemsChanged();
    emit currentWaypointPathChanged();
474
    emit waypointPathChanged();
475

476
    return true;
477 478
}

479
bool WimaController::_setStartIndex()
480
{
481
    bool value;
482
    _currentManager->setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt(&value)-1);
483 484 485 486 487
    Q_ASSERT(value);
    (void)value;

    if ( !_currentManager->update() ) {
        Q_ASSERT(false);
488 489
        return false;
    }
490

491
    emit missionItemsChanged();
492
    emit currentMissionItemsChanged();
493 494
    emit currentWaypointPathChanged();
    emit waypointPathChanged();
495 496

    return true;
497
}
498

499
void WimaController::_recalcCurrentPhase()
500
{
501 502
    if ( !_currentManager->update() ) {
        Q_ASSERT(false);
503
    }
504

505 506 507
    emit missionItemsChanged();
    emit currentMissionItemsChanged();
    emit currentWaypointPathChanged();
508 509
    emit waypointPathChanged();
}
510 511

void WimaController::_updateOverlap()
512
{
513
    bool value;
514
    _currentManager->setOverlap(_overlapWaypoints.rawValue().toUInt(&value));
515 516
    Q_ASSERT(value);
    (void)value;
517

518
    if ( !_currentManager->update() ) {
519 520 521 522 523
        assert(false);
    }

    emit missionItemsChanged();
    emit currentMissionItemsChanged();
524
    emit currentWaypointPathChanged();
525
    emit waypointPathChanged();
526
}
527

528
void WimaController::_updateMaxWaypoints()
529
{
530
    bool value;
531
    _currentManager->setN(_maxWaypointsPerPhase.rawValue().toUInt(&value));
532 533
    Q_ASSERT(value);
    (void)value;
534

535 536
    if ( !_currentManager->update() ) {
        Q_ASSERT(false);
537
    }
538

539 540 541 542
    emit missionItemsChanged();
    emit currentMissionItemsChanged();
    emit currentWaypointPathChanged();
    emit waypointPathChanged();
543 544
}

Valentin Platzgummer's avatar
Valentin Platzgummer committed
545
void WimaController::_updateflightSpeed()
546
{
547
    bool value;
548
    _managerSettings.setFlightSpeed(_flightSpeed.rawValue().toDouble(&value));
549 550 551 552 553 554
    Q_ASSERT(value);
    (void)value;

    if ( !_currentManager->update() ) {
        Q_ASSERT(false);
    }
555

556 557 558 559
    emit missionItemsChanged();
    emit currentMissionItemsChanged();
    emit currentWaypointPathChanged();
    emit waypointPathChanged();
560 561
}

Valentin Platzgummer's avatar
Valentin Platzgummer committed
562
void WimaController::_updateArrivalReturnSpeed()
563
{
564
    bool value;
565
    _managerSettings.setArrivalReturnSpeed(_arrivalReturnSpeed.rawValue().toDouble(&value));
566 567 568 569 570 571
    Q_ASSERT(value);
    (void)value;

    if ( !_currentManager->update() ) {
        Q_ASSERT(false);
    }
572

573 574 575 576
    emit missionItemsChanged();
    emit currentMissionItemsChanged();
    emit currentWaypointPathChanged();
    emit waypointPathChanged();
577 578
}

Valentin Platzgummer's avatar
Valentin Platzgummer committed
579
void WimaController::_updateAltitude()
580
{
581
    bool value;
582
    _managerSettings.setAltitude(_altitude.rawValue().toDouble(&value));
583 584 585 586 587 588
    Q_ASSERT(value);
    (void)value;

    if ( !_currentManager->update() ) {
        Q_ASSERT(false);
    }
589 590 591 592 593

    emit missionItemsChanged();
    emit currentMissionItemsChanged();
    emit currentWaypointPathChanged();
    emit waypointPathChanged();
594 595
}

Valentin Platzgummer's avatar
Valentin Platzgummer committed
596
void WimaController::_checkBatteryLevel()
597
{
598 599 600 601 602
    Vehicle *managerVehicle     = masterController()->managerVehicle();
    WimaSettings* wimaSettings  = qgcApp()->toolbox()->settingsManager()->wimaSettings();
    int batteryThreshold        = wimaSettings->lowBatteryThreshold()->rawValue().toInt();
    bool enabled                = _enableWimaController.rawValue().toBool();
    unsigned int minTime        = wimaSettings->minimalRemainingMissionTime()->rawValue().toUInt();
603

Valentin Platzgummer's avatar
Valentin Platzgummer committed
604
    if (managerVehicle != nullptr && enabled == true) {
605 606 607 608 609 610
        Fact *battery1percentRemaining = managerVehicle->battery1FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName);
        Fact *battery2percentRemaining = managerVehicle->battery2FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName);


        if (battery1percentRemaining->rawValue().toDouble() < batteryThreshold
                && battery2percentRemaining->rawValue().toDouble() < batteryThreshold) {
611 612 613
            if (!_lowBatteryHandlingTriggered) {                
                _lowBatteryHandlingTriggered = true;
                if ( !(_missionController->remainingTime() <= minTime) ) {
614
                    requestSmartRTL();
615 616 617 618 619 620 621 622 623 624
                }
            }
        }
        else {
            _lowBatteryHandlingTriggered = false;
        }

    }
}

Valentin Platzgummer's avatar
Valentin Platzgummer committed
625 626 627
void WimaController::_eventTimerHandler()
{
    static EventTicker batteryLevelTicker(EVENT_TIMER_INTERVAL, CHECK_BATTERY_INTERVAL);
628
    static EventTicker snakeTicker(EVENT_TIMER_INTERVAL, SNAKE_EVENT_LOOP_INTERVAL);
629
    static EventTicker nemoStatusTicker(EVENT_TIMER_INTERVAL, 5000);
Valentin Platzgummer's avatar
Valentin Platzgummer committed
630 631

    // Battery level check necessary?
632 633
    Fact *enableLowBatteryHandling = qgcApp()->toolbox()->settingsManager()->wimaSettings()->enableLowBatteryHandling();
    if ( enableLowBatteryHandling->rawValue().toBool() && batteryLevelTicker.ready() )
Valentin Platzgummer's avatar
Valentin Platzgummer committed
634 635 636
        _checkBatteryLevel();

    // Snake flight plan update necessary?
637 638 639 640
//    if ( snakeEventLoopTicker.ready() ) {
//        if ( _enableSnake.rawValue().toBool() && _localPlanDataValid && !_snakeCalcInProgress && _scenarioDefinedBool) {
//        }
//    }
641

642 643 644 645 646 647
    if ( nemoStatusTicker.ready() ) {
        this->_nemoHeartbeat.setStatus(_fallbackStatus);
        emit WimaController::nemoStatusChanged();
        emit WimaController::nemoStatusStringChanged();
    }

648 649 650 651 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713
    if ( snakeTicker.ready() ) {
        if ( _enableSnake.rawValue().toBool()
             && _pRosBridge->connected() ) {
            if ( !_bridgeSetupDone ) {
                qWarning() << "ROS Bridge setup. ";
                auto start = std::chrono::high_resolution_clock::now();
                _pRosBridge->start();
                auto end = std::chrono::high_resolution_clock::now();
                qWarning() << "_pRosBridge->start() time: "
                           << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
                           << " ms";
                start = std::chrono::high_resolution_clock::now();
                _pRosBridge->publish(_snakeOrigin, "/snake/origin");
                end = std::chrono::high_resolution_clock::now();
                qWarning() << "_pRosBridge->publish(_snakeOrigin, \"/snake/origin\") time: "
                           << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
                           << " ms";
                start = std::chrono::high_resolution_clock::now();
                _pRosBridge->publish(_snakeTilesLocal, "/snake/tiles");
                end = std::chrono::high_resolution_clock::now();
                qWarning() << "_pRosBridge->publish(_snakeTilesLocal, \"/snake/tiles\") time: "
                           << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
                           << " ms";


                start = std::chrono::high_resolution_clock::now();
                _pRosBridge->subscribe("/nemo/progress",
                                       /* callback */ [this](JsonDocUPtr pDoc){
                    int requiredSize = this->_snakeTilesLocal.polygons().size();
                    auto& progress = this->_nemoProgress;
                    if ( !this->_pRosBridge->casePacker()->unpack(pDoc, progress)
                         || progress.progress().size() != requiredSize ) {
                        progress.progress().fill(0, requiredSize);
                    }

                    emit WimaController::nemoProgressChanged();
                });
                end = std::chrono::high_resolution_clock::now();
                qWarning() << "_pRosBridge->subscribe(\"/nemo/progress\" time: "
                           << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
                           << " ms";

                start = std::chrono::high_resolution_clock::now();
                _pRosBridge->subscribe("/nemo/heartbeat",
                                       /* callback */ [this, &nemoStatusTicker](JsonDocUPtr pDoc){
                    if ( !this->_pRosBridge->casePacker()->unpack(pDoc, this->_nemoHeartbeat) ) {
                        if ( this->_nemoHeartbeat.status() == this->_fallbackStatus )
                            return;
                        this->_nemoHeartbeat.setStatus(this->_fallbackStatus);
                    }

                    nemoStatusTicker.reset();
                    this->_fallbackStatus = -1; /*Timeout*/
                    emit WimaController::nemoStatusChanged();
                    emit WimaController::nemoStatusStringChanged();
                });
                end = std::chrono::high_resolution_clock::now();
                qWarning() << "_pRosBridge->subscribe(\"/nemo/heartbeat\" time: "
                           << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
                           << " ms";
                _bridgeSetupDone = true;
            }
        } else if ( _bridgeSetupDone) {
            _pRosBridge->reset();
            _bridgeSetupDone = false;
        }
714
    }
Valentin Platzgummer's avatar
Valentin Platzgummer committed
715 716 717
}

void WimaController::_smartRTLCleanUp(bool flying)
718
{
719 720 721 722 723
    if ( !flying) { // vehicle has landed
        _switchWaypointManager(_defaultManager);
        _missionController->removeAllFromVehicle();
        _missionController->removeAll();
        disconnect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp);
724 725 726
    }
}

727 728
void WimaController::_setPhaseDistance(double distance)
{
729 730 731
    (void)distance;
//    if (!qFuzzyCompare(distance, _phaseDistance)) {
//        _phaseDistance = distance;
732

733 734
//        emit phaseDistanceChanged();
//    }
735 736 737 738
}

void WimaController::_setPhaseDuration(double duration)
{
739 740 741
    (void)duration;
//    if (!qFuzzyCompare(duration, _phaseDuration)) {
//        _phaseDuration = duration;
742

743 744
//        emit phaseDurationChanged();
//    }
745 746
}

747
bool WimaController::_checkSmartRTLPreCondition(QString &errorString)
748 749
{
    if (!_localPlanDataValid) {
750 751
        errorString.append(tr("No WiMA data available. Please define at least a measurement and a service area."));
        return false;
752
    }
753

754
    return _rtlManager.checkPrecondition(errorString);
755 756
}

757
void WimaController::_switchWaypointManager(WaypointManager::ManagerBase &manager)
758
{
759 760 761
    if (_currentManager != &manager) {
        _currentManager = &manager;

Valentin Platzgummer's avatar
Valentin Platzgummer committed
762 763 764 765 766 767 768 769 770 771 772 773 774 775 776 777 778
        disconnect(&_overlapWaypoints,             &Fact::rawValueChanged,
                   this, &WimaController::_updateOverlap);
        disconnect(&_maxWaypointsPerPhase,         &Fact::rawValueChanged,
                   this, &WimaController::_updateMaxWaypoints);
        disconnect(&_nextPhaseStartWaypointIndex,  &Fact::rawValueChanged,
                   this, &WimaController::_setStartIndex);

        _maxWaypointsPerPhase.setRawValue(_currentManager->N());
        _overlapWaypoints.setRawValue(_currentManager->overlap());
        _nextPhaseStartWaypointIndex.setRawValue(_currentManager->startIndex());

        connect(&_overlapWaypoints,             &Fact::rawValueChanged,
                this, &WimaController::_updateOverlap);
        connect(&_maxWaypointsPerPhase,         &Fact::rawValueChanged,
                this, &WimaController::_updateMaxWaypoints);
        connect(&_nextPhaseStartWaypointIndex,  &Fact::rawValueChanged,
                this, &WimaController::_setStartIndex);
779

780 781 782 783
        emit missionItemsChanged();
        emit currentMissionItemsChanged();
        emit waypointPathChanged();
        emit currentWaypointPathChanged();
784

785
        qWarning() << "WimaController::_switchWaypointManager: statistics update missing.";
786 787 788
    }
}

789 790
void WimaController::_initSmartRTL()
{
791 792 793 794
    QString errorString;
    static int attemptCounter = 0;
    attemptCounter++;

795
    if ( _checkSmartRTLPreCondition(errorString) ) {
796 797 798 799 800 801 802 803 804 805 806 807 808 809 810
        _masterController->managerVehicle()->pauseVehicle();
        connect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp);
        if ( _rtlManager.update() ) { // Calculate return path.
            _switchWaypointManager(_rtlManager);
            attemptCounter = 0;
            emit smartRTLPathConfirm();
            return;
        }
    } else if (attemptCounter > SMART_RTL_MAX_ATTEMPTS) {
        errorString.append(tr("Smart RTL: No success after maximum number of attempts."));
        qgcApp()->showMessage(errorString);
        attemptCounter = 0;
    } else {
        _smartRTLTimer.singleShot(SMART_RTL_ATTEMPT_INTERVAL, this, &WimaController::_initSmartRTL);
    }
811 812
}

Valentin Platzgummer's avatar
Valentin Platzgummer committed
813 814 815 816 817 818 819 820
void WimaController::_setSnakeCalcInProgress(bool inProgress)
{
    if (_snakeCalcInProgress != inProgress){
        _snakeCalcInProgress = inProgress;
        emit snakeCalcInProgressChanged();
    }
}

821 822
void WimaController::_snakeStoreWorkerResults()
{
823
    auto start = std::chrono::high_resolution_clock::now();
824 825
    _snakeManager.clear();

Valentin Platzgummer's avatar
Valentin Platzgummer committed
826
    const WorkerResult_t &r = _snakeWorker.result();
827 828
    _setSnakeCalcInProgress(false);
    if (!r.success) {
829
        //qgcApp()->showMessage(r.errorMessage);
830 831 832 833 834
        return;
    }

    // create Mission items from r.waypoints
    long n = r.waypoints.size() - r.returnPathIdx.size() - r.arrivalPathIdx.size() + 2;
835
    if (n < 1) {
836 837
        return;
    }
838 839 840

    // Create QVector<QGeoCoordinate> containing all waypoints;
    unsigned long startIdx = r.arrivalPathIdx.last();
841 842
    unsigned  long endIdx = r.returnPathIdx.first();
    for (unsigned long i = startIdx; i <= endIdx; ++i) {
Valentin Platzgummer's avatar
Valentin Platzgummer committed
843
        _snakeManager.push_back(r.waypoints[int(i)]);
844 845
    }

846
    _snakeManager.update();
847

848
    emit missionItemsChanged();
849 850 851
    emit currentMissionItemsChanged();
    emit currentWaypointPathChanged();
    emit waypointPathChanged();
852 853 854 855

    auto end = std::chrono::high_resolution_clock::now();
    double duration = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
    qWarning() << "WimaController::_snakeStoreWorkerResults execution time: " << duration << " ms.";
Valentin Platzgummer's avatar
Valentin Platzgummer committed
856 857
}

858 859 860 861 862 863 864 865 866 867 868
void WimaController::_initStartSnakeWorker()
{
    if ( !_enableSnake.rawValue().toBool() )
        return;

    // Stop worker thread if running.
    if ( _snakeWorker.isRunning() ) {
        _snakeWorker.quit();
    }

    // Initialize _snakeWorker.
Valentin Platzgummer's avatar
Valentin Platzgummer committed
869 870 871 872 873 874 875 876 877 878 879 880 881 882 883 884 885 886
    _snakeWorker.setMeasurementArea(
                _measurementArea.coordinateList());
    _snakeWorker.setServiceArea(
                _serviceArea.coordinateList());
    _snakeWorker.setCorridor(
                _corridor.coordinateList());
    _snakeWorker.setProgress(
                _nemoProgress.progress());
    _snakeWorker.setLineDistance(
                _snakeLineDistance.rawValue().toDouble());
    _snakeWorker.setMinTransectLength(
                _snakeMinTransectLength.rawValue().toDouble());
    _snakeWorker.setTileHeight(
                _snakeTileHeight.rawValue().toDouble());
    _snakeWorker.setTileWidth(
                _snakeTileWidth.rawValue().toDouble());
    _snakeWorker.setMinTileArea(
                _snakeMinTileArea.rawValue().toDouble());
887 888 889 890 891 892
    _setSnakeCalcInProgress(true);

    // Start worker thread.
    _snakeWorker.start();
}

893 894 895 896 897 898 899 900
void WimaController::_switchSnakeManager(QVariant variant)
{
    if (variant.value<bool>()){
        _switchWaypointManager(_snakeManager);
    } else {
        _switchWaypointManager(_defaultManager);
    }
}