WimaController.cc

#include "WimaController.h"

#include "utilities.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 "ros_bridge/include/messages/geographic_msgs/geopoint.h"
#include "ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h"
#include "ros_bridge/include/messages/nemo_msgs/progress.h"
#include "ros_bridge/include/messages/nemo_msgs/heartbeat.h"

#include "Snake/QNemoProgress.h"
#include "Snake/QNemoHeartbeat.h"

#include "QVector3D"
#include <QScopedPointer>

#include <memory>

#define SMART_RTL_MAX_ATTEMPTS 3 // times
#define SMART_RTL_ATTEMPT_INTERVAL 200 // ms
#define EVENT_TIMER_INTERVAL 50 // ms



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::snakeTileWidthName          = "SnakeTileWidth";
const char* WimaController::snakeTileHeightName         = "SnakeTileHeight";
const char* WimaController::snakeMinTileAreaName        = "SnakeMinTileArea";
const char* WimaController::snakeLineDistanceName       = "SnakeLineDistance";
const char* WimaController::snakeMinTransectLengthName  = "SnakeMinTransectLength";

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

using namespace snake;
using namespace snake_geometry;

WimaController::WimaController(QObject *parent)
    : QObject                   (parent)
    , _joinedArea               ()
    , _measurementArea          ()
    , _serviceArea              ()
    , _corridor                 ()
    , _localPlanDataValid       (false)
    , _areaInterface            (&_measurementArea, &_serviceArea, &_corridor, &_joinedArea)
    , _managerSettings          ()
    , _defaultManager           (_managerSettings, _areaInterface)
    , _snakeManager             (_managerSettings, _areaInterface)
    , _rtlManager               (_managerSettings, _areaInterface)
    , _currentManager           (&_defaultManager)
    , _managerList              {&_defaultManager, &_snakeManager, &_rtlManager}
    , _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])
    , _snakeTileWidth           (settingsGroup, _metaDataMap[snakeTileWidthName])
    , _snakeTileHeight          (settingsGroup, _metaDataMap[snakeTileHeightName])
    , _snakeMinTileArea         (settingsGroup, _metaDataMap[snakeMinTileAreaName])
    , _snakeLineDistance        (settingsGroup, _metaDataMap[snakeLineDistanceName])
    , _snakeMinTransectLength   (settingsGroup, _metaDataMap[snakeMinTransectLengthName])
    , _lowBatteryHandlingTriggered  (false)
    , _measurementPathLength    (-1)
    , _snakeCalcInProgress      (false)
    , _nemoHeartbeat            (0 /*status: not connected*/)
    , _fallbackStatus           (0 /*status: not connected*/)
    , _batteryLevelTicker       (EVENT_TIMER_INTERVAL, 1000 /*ms*/)
    , _snakeTicker              (EVENT_TIMER_INTERVAL, 200 /*ms*/)
    , _nemoTimeoutTicker        (EVENT_TIMER_INTERVAL, 5000 /*ms*/)
    , _topicServiceSetupDone    (false)
{

    // Set up facts.
    _showAllMissionItems.setRawValue(true);
    _showCurrentMissionItems.setRawValue(true);
    connect(&_overlapWaypoints,             &Fact::rawValueChanged, this, &WimaController::_updateOverlap);
    connect(&_maxWaypointsPerPhase,         &Fact::rawValueChanged, this, &WimaController::_updateMaxWaypoints);
    connect(&_nextPhaseStartWaypointIndex,  &Fact::rawValueChanged, this, &WimaController::_setStartIndex);
    connect(&_flightSpeed,                  &Fact::rawValueChanged, this, &WimaController::_updateflightSpeed);
    connect(&_arrivalReturnSpeed,           &Fact::rawValueChanged, this, &WimaController::_updateArrivalReturnSpeed);
    connect(&_altitude,                     &Fact::rawValueChanged, this, &WimaController::_updateAltitude);

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

    // Periodic.
    connect(&_eventTimer, &QTimer::timeout, this, &WimaController::_eventTimerHandler);
    //_eventTimer.setInterval(EVENT_TIMER_INTERVAL);
    _eventTimer.start(EVENT_TIMER_INTERVAL);

    // Snake Worker Thread.
    connect(&_snakeDataManager, &SnakeDataManager::finished, this, &WimaController::_snakeStoreWorkerResults);
    connect(this, &WimaController::nemoProgressChanged, this, &WimaController::_initStartSnakeWorker);
    connect(this, &QObject::destroyed, &this->_snakeDataManager, &SnakeWorker::quit);

    // Snake.
    connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_initStartSnakeWorker);
    _initStartSnakeWorker();
    connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_switchSnakeManager);
    _switchSnakeManager(_enableSnake.rawValue());
}

PlanMasterController *WimaController::masterController() {
    return _masterController;
}

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

QmlObjectListModel *WimaController::visualItems() {
    return &_areas;
}

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

QmlObjectListModel *WimaController::currentMissionItems() {
    return const_cast<QmlObjectListModel*>(&_currentManager->currentMissionItems());
}

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

QVariantList WimaController::currentWaypointPath()
{
    return const_cast<QVariantList&>(_currentManager->currentWaypointsVariant());
}

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;