WimaController.cc

#include "WimaController.h"
#include "utilities.h"
#include "ros_bridge/include/JsonMethodes.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h"

#include "QtROSJsonFactory.h"
#include "QtROSTypeFactory.h"

#include "time.h"
#include "assert.h"

#include "QVector3D"
#include <QScopedPointer>


const char* WimaController::wimaFileExtension           = "wima";
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";
const char* WimaController::flightSpeedName             = "FlightSpeed";
const char* WimaController::arrivalReturnSpeedName      = "ArrivalReturnSpeed";
const char* WimaController::altitudeName                = "Altitude";
const char* WimaController::reverseName                 = "Reverse";
const char* WimaController::snakeTileWidthName          = "SnakeTileWidth";
const char* WimaController::snakeTileHeightName         = "SnakeTileHeight";
const char* WimaController::snakeMinTileAreaName        = "SnakeMinTileArea";
const char* WimaController::snakeLineDistanceName       = "SnakeLineDistance";
const char* WimaController::snakeMinTransectLengthName  = "SnakeMinTransectLength";

using namespace snake;
using namespace snake_geometry;

WimaController::WimaController(QObject *parent)
    : QObject                   (parent)
    , _container                (nullptr)
    , _joinedArea               (this)
    , _measurementArea          (this)
    , _serviceArea              (this)
    , _corridor                 (this)
    , _localPlanDataValid       (false)
    , _metaDataMap              (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/WimaController.SettingsGroup.json"), this))
    , _enableWimaController     (settingsGroup, _metaDataMap[enableWimaControllerName])
    , _overlapWaypoints         (settingsGroup, _metaDataMap[overlapWaypointsName])
    , _maxWaypointsPerPhase     (settingsGroup, _metaDataMap[maxWaypointsPerPhaseName])
    , _nextPhaseStartWaypointIndex       (settingsGroup, _metaDataMap[startWaypointIndexName])
    , _showAllMissionItems      (settingsGroup, _metaDataMap[showAllMissionItemsName])
    , _showCurrentMissionItems  (settingsGroup, _metaDataMap[showCurrentMissionItemsName])    
    , _flightSpeed              (settingsGroup, _metaDataMap[flightSpeedName])
    , _arrivalReturnSpeed       (settingsGroup, _metaDataMap[arrivalReturnSpeedName])
    , _altitude                 (settingsGroup, _metaDataMap[altitudeName])
    , _reverse                  (settingsGroup, _metaDataMap[reverseName])
    , _endWaypointIndex         (0)
    , _startWaypointIndex       (0)
    , _uploadOverrideRequired   (false)
    , _measurementPathLength    (-1)
    , _arrivalPathLength        (-1)
    , _returnPathLength         (-1)
    , _phaseDistance            (-1)
    , _phaseDuration            (-1)
    , _phaseDistanceBuffer      (-1)
    , _phaseDurationBuffer      (-1)
    , _vehicleHasLowBattery         (false)
    , _lowBatteryHandlingTriggered  (false)
    , _executingSmartRTL            (false)
    , _snakeConnectionStatus    (SnakeConnectionStatus::Connected) // TODO: implement automatic connection
    , _snakeCalcInProgress      (false)
    , _scenarioDefinedBool      (false)
    , _snakeTileWidth           (settingsGroup, _metaDataMap[snakeTileWidthName])
    , _snakeTileHeight          (settingsGroup, _metaDataMap[snakeTileHeightName])
    , _snakeMinTileArea         (settingsGroup, _metaDataMap[snakeMinTileAreaName])
    , _snakeLineDistance        (settingsGroup, _metaDataMap[snakeLineDistanceName])
    , _snakeMinTransectLength   (settingsGroup, _metaDataMap[snakeMinTransectLengthName])
{
    _showAllMissionItems.setRawValue(true);
    _showCurrentMissionItems.setRawValue(true);
    connect(&_overlapWaypoints,             &Fact::rawValueChanged, this, &WimaController::_updateNextWaypoint);
    connect(&_maxWaypointsPerPhase,         &Fact::rawValueChanged, this, &WimaController::_recalcCurrentPhase);
    connect(&_nextPhaseStartWaypointIndex,  &Fact::rawValueChanged, this, &WimaController::_calcNextPhase);
    connect(&_flightSpeed,                  &Fact::rawValueChanged, this, &WimaController::_updateflightSpeed);
    connect(&_arrivalReturnSpeed,           &Fact::rawValueChanged, this, &WimaController::_updateArrivalReturnSpeed);
    connect(&_altitude,                     &Fact::rawValueChanged, this, &WimaController::_updateAltitude);
    connect(&_reverse,                      &Fact::rawValueChanged, this, &WimaController::_reverseChangedHandler);

    // setup low battery handling
    connect(&_eventTimer, &QTimer::timeout, this, &WimaController::_eventTimerHandler);
    _eventTimer.setInterval(EVENT_TIMER_INTERVAL);

    Fact *enableLowBatteryHandling = qgcApp()->toolbox()->settingsManager()->wimaSettings()->enableLowBatteryHandling();
    connect(enableLowBatteryHandling, &Fact::rawValueChanged, this, &WimaController::_enableDisableLowBatteryHandling);
    _enableDisableLowBatteryHandling(enableLowBatteryHandling->rawValue());

    // Snake Worker Thread.
    connect(&_snakeWorker, &SnakeWorker::resultReady, this, &WimaController::_snakeStoreWorkerResults);
}

QStringList WimaController::loadNameFilters() const
{
    QStringList filters;

    filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension) <<
               tr("All Files (*.*)");
    return filters;
}

QStringList WimaController::saveNameFilters() const
{
    QStringList filters;

    filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension);
    return filters;
}

bool WimaController::uploadOverrideRequired() const
{
    return _uploadOverrideRequired;
}

double WimaController::phaseDistance() const
{
    return _phaseDistance;
}

double WimaController::phaseDuration() const
{
    return _phaseDuration;
}

bool WimaController::vehicleHasLowBattery() const
{
    return _vehicleHasLowBattery;
}

long WimaController::snakeConnectionStatus() const
{
    return _snakeConnectionStatus;
}

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

void WimaController::setMasterController(PlanMasterController *masterC)
{
    _masterController = masterC;
    emit masterControllerChanged();
}

void WimaController::setMissionController(MissionController *missionC)
{
    _missionController = missionC;
    emit missionControllerChanged();
}

/*!
 * \fn void WimaController::setDataContainer(WimaDataContainer *container)
 * Sets the pointer to the \c WimaDataContainer, which is meant to exchange data between the \c WimaController and the \c WimaPlaner.
 *
 * \sa WimaPlaner, WimaDataContainer, WimaPlanData
 */
void WimaController::setDataContainer(WimaDataContainer *container)
{
    if (container != nullptr) {
        if (_container != nullptr) {
           disconnect(_container, &WimaDataContainer::newDataAvailable, this, &WimaController::_fetchContainerData);
        }

        _container = container;
        connect(_container, &WimaDataContainer::newDataAvailable, this, &WimaController::_fetchContainerData);

        emit dataContainerChanged();
    }
}

void WimaController::setUploadOverrideRequired(bool overrideRequired)
{
    if (_uploadOverrideRequired != overrideRequired) {
        _uploadOverrideRequired = overrideRequired;

        emit uploadOverrideRequiredChanged();
    }
}

void WimaController::nextPhase()
{
    _calcNextPhase();
}

void WimaController::previousPhase()
{    
    bool reverseBool = _reverse.rawValue().toBool();
    if (!reverseBool){