before restoring src/comm/ros_bridge/include

qgroundcontrol.pro

@@ -489,7 +489,7 @@ HEADERS += \
     src/comm/ros_bridge/include/MessageTag.h \
     src/comm/ros_bridge/include/MessageTraits.h \
     src/comm/ros_bridge/include/RosBridgeClient.h \
-    src/comm/ros_bridge/include/ThreadSafeQueue.h \
+    src/comm/ros_bridge/include/Server.h \
     src/comm/ros_bridge/include/TopicPublisher.h \
     src/comm/ros_bridge/include/TopicSubscriber.h \
     src/comm/ros_bridge/include/TypeFactory.h \
@@ -513,6 +513,7 @@ SOURCES += \
     src/Wima/WimaBridge.cc \
     src/comm/ros_bridge/include/ComPrivateInclude.cpp \
     src/comm/ros_bridge/include/MessageTag.cpp \
+    src/comm/ros_bridge/include/Server.cpp \
     src/comm/ros_bridge/include/TopicPublisher.cpp \
     src/comm/ros_bridge/include/TopicSubscriber.cpp \
     src/comm/ros_bridge/src/CasePacker.cpp \

src/Wima/WimaController.cc

@@ -35,9 +35,10 @@ 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")};
+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;
@@ -55,7 +56,9 @@ WimaController::WimaController(QObject *parent)
     , _snakeManager             (_managerSettings, _areaInterface)
     , _rtlManager               (_managerSettings, _areaInterface)
     , _currentManager           (&_defaultManager)
-    , _metaDataMap              (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/WimaController.SettingsGroup.json"), this))
+    , _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])
@@ -68,7 +71,6 @@ WimaController::WimaController(QObject *parent)
     , _measurementPathLength    (-1)
     , _lowBatteryHandlingTriggered  (false)
     , _snakeCalcInProgress      (false)
-    , _scenarioDefinedBool      (false)
     , _snakeTileWidth           (settingsGroup, _metaDataMap[snakeTileWidthName])
     , _snakeTileHeight          (settingsGroup, _metaDataMap[snakeTileHeightName])
     , _snakeMinTileArea         (settingsGroup, _metaDataMap[snakeMinTileAreaName])
@@ -89,9 +91,20 @@ WimaController::WimaController(QObject *parent)
     connect(&_arrivalReturnSpeed,           &Fact::rawValueChanged, this, &WimaController::_updateArrivalReturnSpeed);
     connect(&_altitude,                     &Fact::rawValueChanged, this, &WimaController::_updateAltitude);
 
-    _defaultManager.setOverlap(_overlapWaypoints.rawValue().toUInt());
-    _defaultManager.setN(_maxWaypointsPerPhase.rawValue().toUInt());
-    _defaultManager.setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt());
+    // 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);
@@ -343,17 +356,8 @@ bool WimaController::_calcShortestPath(const QGeoCoordinate &start, const QGeoCo
     return  retVal;
 }
 
-/*!
- * \fn void WimaController::containerDataValidChanged(bool valid)
- * Pulls plan data generated by \c WimaPlaner from the \c _container if the data is valid (\a valid equals true).
- * Is connected to the dataValidChanged() signal of the \c WimaDataContainer.
- *
- * \sa WimaDataContainer, WimaPlaner, WimaPlanData
- */
 bool WimaController::setWimaPlanData(const WimaPlanData &planData)
 {
-    // fetch only if valid, return true on success
-
     // reset visual items
     _areas.clear();
     _defaultManager.clear();
@@ -779,17 +783,6 @@ void WimaController::_eventTimerHandler()
             _pRosBridge->reset();
             _bridgeSetupDone = false;
         }
-
-        //qWarning() << "Connectable: " << _pRosBridge->connected() << "\n";
-
-
-        //auto start = std::chrono::high_resolution_clock::now();
-
-
-
-        //auto end = std::chrono::high_resolution_clock::now();
-        //qWarning() << "Duration: " << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
-                   //<< " ms\n";
     }
 }
 
@@ -838,14 +831,23 @@ void WimaController::_switchWaypointManager(WaypointManager::ManagerBase &manage
     if (_currentManager != &manager) {
         _currentManager = &manager;
 
-        bool value;
-        _currentManager->setN(_maxWaypointsPerPhase.rawValue().toUInt(&value));
-        Q_ASSERT(value);
-        _currentManager->setOverlap(_overlapWaypoints.rawValue().toUInt(&value));
-        Q_ASSERT(value);
-        _currentManager->setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt(&value)-1);
-        Q_ASSERT(value);
-        (void)value;
+        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);
 
         emit missionItemsChanged();
         emit currentMissionItemsChanged();
@@ -888,33 +890,13 @@ void WimaController::_setSnakeCalcInProgress(bool inProgress)
     }
 }
 
-bool WimaController::_isScenarioDefined()
-{
-    _scenarioDefinedBool = _scenario.defined(_snakeTileWidth.rawValue().toDouble(),
-                                             _snakeTileHeight.rawValue().toDouble(),
-                                             _snakeMinTileArea.rawValue().toDouble());
-    return _scenarioDefinedBool;
-}
-
-bool WimaController::_isScenarioDefinedErrorMessage()
-{
-    bool value = _isScenarioDefined();
-    if (!value){
-        QString errorString;
-        for (auto c : _scenario.errorString)
-            errorString.push_back(c);
-        qgcApp()->showMessage(errorString);
-    }
-    return value;
-}
-
 void WimaController::_snakeStoreWorkerResults()
 {
+    _setSnakeCalcInProgress(false);
     auto start = std::chrono::high_resolution_clock::now();
     _snakeManager.clear();
 
     const WorkerResult_t &r = _snakeWorker.getResult();
-    _setSnakeCalcInProgress(false);
     if (!r.success) {
         //qgcApp()->showMessage(r.errorMessage);
         return;

src/Wima/WimaController.h

@@ -338,8 +338,6 @@ private slots:
     void    _smartRTLCleanUp                    (bool flying);
     // Snake.
     void _setSnakeCalcInProgress             (bool inProgress);
-    bool _isScenarioDefined                  (void);
-    bool _isScenarioDefinedErrorMessage      (void);
     void _snakeStoreWorkerResults            ();
     void _startStopRosBridge                 ();
     void _initStartSnakeWorker               ();
@@ -357,7 +355,6 @@ private:
     MissionController      *_missionController;
 
     // Wima Data.
-
     QmlObjectListModel      _areas; // contains all visible areas
     WimaJoinedAreaData      _joinedArea; // joined area fromed by opArea, serArea, _corridor
     WimaMeasurementAreaData _measurementArea; // measurement area
@@ -372,6 +369,8 @@ private:
     WaypointManager::DefaultManager     _snakeManager;
     WaypointManager::RTLManager         _rtlManager;
     WaypointManager::ManagerBase       *_currentManager;
+    using ManagerList = QList<WaypointManager::ManagerBase*>;
+    ManagerList                         _managerList;
 
     // Settings Facts.
     QMap<QString, FactMetaData*> _metaDataMap;

src/Wima/WimaController_new.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 "ros_bridge/include/CasePacker.h"
+
+#include "Snake/QtROSJsonFactory.h"
+#include "Snake/QtROSTypeFactory.h"
+#include "Snake/QNemoProgress.h"
+#include "Snake/QNemoHeartbeat.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::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])
+    , _measurementPathLength    (-1)
+    , _lowBatteryHandlingTriggered  (false)
+    , _snakeCalcInProgress      (false)
+    , _snakeTileWidth           (settingsGroup, _metaDataMap[snakeTileWidthName])
+    , _snakeTileHeight          (settingsGroup, _metaDataMap[snakeTileHeightName])
+    , _snakeMinTileArea         (settingsGroup, _metaDataMap[snakeMinTileAreaName])
+    , _snakeLineDistance        (settingsGroup, _metaDataMap[snakeLineDistanceName])
+    , _snakeMinTransectLength   (settingsGroup, _metaDataMap[snakeMinTransectLengthName])
+    , _nemoHeartbeat            (0 /*status: not connected*/)
+    , _fallbackStatus           (0 /*status: not connected*/)
+    , _bridgeSetupDone          (false)
+    , _pRosBridge               (new ROSBridge::ROSBridge())
+{
+    // 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(&_snakeWorker, &SnakeDataManager::finished, this, &WimaController::_updateSnakeData);
+    connect(this, &WimaController::nemoProgressChanged, this, &WimaController::_initStartSnakeWorker);
+    connect(this, &QObject::destroyed, &this->_snakeWorker, &SnakeDataManager::quit);
+
+    // Snake.
+    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;
+}
+
+QVector<int> WimaController::nemoProgress() {
+    if ( _nemoProgress.progress().size() == _snakeTileCenterPoints.size() )
+        return _nemoProgress.progress();
+    else
+        return QVector<int>(_snakeTileCenterPoints.size(), 0);
+}
+
+double WimaController::phaseDistance() const
+{
+    return 0.0;
+}
+
+double WimaController::phaseDuration() const
+{
+    return 0.0;
+}
+
+int WimaController::nemoStatus() const
+{
+    return _nemoHeartbeat.status();
+}
+
+QString WimaController::nemoStatusString() const
+{
+    return _nemoStatusMap.at(_nemoHeartbeat.status());
+}
+
+bool WimaController::snakeCalcInProgress() const
+{
+    return _snakeCalcInProgress;
+}
+
+void WimaController::setMasterController(PlanMasterController *masterC)
+{
+    _masterController = masterC;
+    _managerSettings.setMasterController(masterC);
+    emit masterControllerChanged();
+}
+
+void WimaController::setMissionController(MissionController *missionC)
+{
+    _missionController = missionC;
+    _managerSettings.setMissionController(missionC);
+    emit missionControllerChanged();
+}
+
+void WimaController::nextPhase()
+{
+    _calcNextPhase();
+}
+
+void WimaController::previousPhase()
+{        
+    if ( !_currentManager->previous() ) {
+        Q_ASSERT(false);
+    }
+
+    emit missionItemsChanged();
+    emit currentMissionItemsChanged();
+    emit currentWaypointPathChanged();
+    emit waypointPathChanged();
+}
+
+void WimaController::resetPhase()
+{
+    if ( !_currentManager->reset() ) {
+        Q_ASSERT(false);
+    }
+
+    emit missionItemsChanged();
+    emit currentMissionItemsChanged();
+    emit currentWaypointPathChanged();
+    emit waypointPathChanged();
+}
+
+void WimaController::requestSmartRTL()
+{
+    QString errorString("Smart RTL requested. ");
+    if ( !_checkSmartRTLPreCondition(errorString) ){
+        qgcApp()->showMessage(errorString);
+        return;
+    }
+    emit smartRTLRequestConfirm();
+}
+
+bool WimaController::upload()
+{
+    auto &currentMissionItems = _defaultManager.currentMissionItems();
+    if (   !_serviceArea.containsCoordinate(_masterController->managerVehicle()->coordinate())
+        && currentMissionItems.count() > 0) {
+        emit forceUploadConfirm();
+        return false;
+    }
+
+    return forceUpload();
+}
+
+bool WimaController::forceUpload()
+{
+    auto &currentMissionItems = _defaultManager.currentMissionItems();
+    if (currentMissionItems.count() < 1)
+        return false;
+
+    _missionController->removeAll();
+    // Set homeposition of settingsItem.
+    QmlObjectListModel* visuals = _missionController->visualItems();
+    MissionSettingsItem* settingsItem = visuals->value<MissionSettingsItem *>(0);
+    if (settingsItem == nullptr) {
+        Q_ASSERT(false);
+        qWarning("WimaController::updateCurrentMissionItems(): nullptr");
+        return false;
+    }
+    settingsItem->setCoordinate(_managerSettings.homePosition());
+
+    // Copy mission items to _missionController.
+    for (int i = 1; i < currentMissionItems.count(); i++){
+        auto *item = currentMissionItems.value<const SimpleMissionItem *>(i);
+        _missionController->insertSimpleMissionItem(*item, visuals->count());
+    }
+
+    _masterController->sendToVehicle();
+
+    return true;
+}
+
+void WimaController::removeFromVehicle()
+{
+    _masterController->removeAllFromVehicle();
+    _missionController->removeAll();
+}
+
+void WimaController::executeSmartRTL()
+{
+    forceUpload();
+    masterController()->managerVehicle()->startMission();
+}
+
+void WimaController::initSmartRTL()
+{
+    _initSmartRTL();
+}
+
+void WimaController::removeVehicleTrajectoryHistory()
+{
+    Vehicle *managerVehicle = masterController()->managerVehicle();
+    managerVehicle->trajectoryPoints()->clear();
+}
+
+bool WimaController::_calcShortestPath(const QGeoCoordinate &start,
+                                       const QGeoCoordinate &destination,
+                                       QVector<QGeoCoordinate> &path)
+{
+    using namespace GeoUtilities;
+    using namespace PolygonCalculus;
+    QPolygonF polygon2D;
+    toCartesianList(_joinedArea.coordinateList(), /*origin*/ start, polygon2D);
+    QPointF start2D(0,0);
+    QPointF end2D;
+    toCartesian(destination, start, end2D);
+    QVector<QPointF> path2D;
+
+    bool retVal = PolygonCalculus::shortestPath(polygon2D, start2D, end2D, path2D);
+    toGeoList(path2D, /*origin*/ start, path);
+
+    return  retVal;
+}
+
+bool WimaController::setWimaPlanData(const WimaPlanData &planData)
+{
+    // reset visual items
+    _areas.clear();
+    _defaultManager.clear();
+    _snakeTiles.polygons().clear();
+    _snakeTilesLocal.polygons().clear();
+    _snakeTileCenterPoints.clear();
+
+    emit visualItemsChanged();
+    emit missionItemsChanged();
+    emit currentMissionItemsChanged();
+    emit waypointPathChanged();
+    emit currentWaypointPathChanged();
+    emit snakeTilesChanged();
+    emit snakeTileCenterPointsChanged();
+
+    _localPlanDataValid = false;
+
+    // extract list with WimaAreas
+    QList<const WimaAreaData*> areaList = planData.areaList();
+
+    int areaCounter = 0;
+    const int numAreas = 4; // extract only numAreas Areas, if there are more they are invalid and ignored
+    for (int i = 0; i < areaList.size(); i++) {
+        const WimaAreaData *areaData = areaList[i];
+
+        if (areaData->type() == WimaServiceAreaData::typeString) { // is it a service area?
+            _serviceArea = *qobject_cast<const WimaServiceAreaData*>(areaData);
+            areaCounter++;
+            _areas.append(&_serviceArea);
+
+            continue;
+        }
+
+        if (areaData->type() == WimaMeasurementAreaData::typeString) { // is it a measurement area?
+            _measurementArea =  *qobject_cast<const WimaMeasurementAreaData*>(areaData);
+            areaCounter++;
+            _areas.append(&_measurementArea);
+
+            continue;
+        }
+
+        if (areaData->type() == WimaCorridorData::typeString) { // is it a corridor?
+            _corridor =  *qobject_cast<const WimaCorridorData*>(areaData);
+            areaCounter++;
+            //_visualItems.append(&_corridor); // not needed
+
+            continue;
+        }
+
+        if (areaData->type() == WimaJoinedAreaData::typeString) { // is it a corridor?
+            _joinedArea =  *qobject_cast<const WimaJoinedAreaData*>(areaData);
+            areaCounter++;
+            _areas.append(&_joinedArea);
+
+            continue;
+        }
+
+        if (areaCounter >= numAreas)
+            break;
+    }
+
+    if (areaCounter != numAreas) {
+        Q_ASSERT(false);
+        return false;
+    }
+
+    emit visualItemsChanged();
+
+    // extract mission items
+    QList<MissionItem> tempMissionItems = planData.missionItems();
+    if (tempMissionItems.size() < 1) {
+        qWarning("WimaController: Mission items from WimaPlaner empty!");
+        return false;
+    }
+
+    for (auto item : tempMissionItems) {
+        _defaultManager.push_back(item.coordinate());
+    }
+
+    _managerSettings.setHomePosition( QGeoCoordinate(_serviceArea.center().latitude(),
+                                                     _serviceArea.center().longitude(),
+                                                     0) );
+
+    if( !_defaultManager.reset() ){
+        Q_ASSERT(false);
+        return false;
+    }
+
+    emit missionItemsChanged();
+    emit currentMissionItemsChanged();
+    emit waypointPathChanged();
+    emit currentWaypointPathChanged();
+
+    _localPlanDataValid = true;
+
+    _initStartSnakeWorker();
+
+    return true;
+}
+
+WimaController *WimaController::thisPointer()
+{
+    return this;
+}
+
+bool WimaController::_calcNextPhase()
+{
+    if ( !_currentManager->next() ) {
+        Q_ASSERT(false);
+        return false;
+    }
+
+    emit missionItemsChanged();
+    emit currentMissionItemsChanged();
+    emit currentWaypointPathChanged();
+    emit waypointPathChanged();
+
+    return true;
+}
+
+bool WimaController::_setStartIndex()
+{
+    bool value;
+    _currentManager->setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt(&value)-1);
+    Q_ASSERT(value);
+    (void)value;
+
+    if ( !_currentManager->update() ) {
+        Q_ASSERT(false);
+        return false;
+    }
+
+    emit missionItemsChanged();
+    emit currentMissionItemsChanged();
+    emit currentWaypointPathChanged();
+    emit waypointPathChanged();
+
+    return true;
+}
+
+void WimaController::_recalcCurrentPhase()
+{
+    if ( !_currentManager->update() ) {
+        Q_ASSERT(false);
+    }
+
+    emit missionItemsChanged();
+    emit currentMissionItemsChanged();
+    emit currentWaypointPathChanged();
+    emit waypointPathChanged();
+}
+
+void WimaController::_updateOverlap()
+{
+    bool value;
+    _currentManager->setOverlap(_overlapWaypoints.rawValue().toUInt(&value));
+    Q_ASSERT(value);
+    (void)value;
+
+    if ( !_currentManager->update() ) {
+        assert(false);
+    }
+
+    emit missionItemsChanged();
+    emit currentMissionItemsChanged();
+    emit currentWaypointPathChanged();
+    emit waypointPathChanged();
+}
+
+void WimaController::_updateMaxWaypoints()
+{
+    bool value;
+    _currentManager->setN(_maxWaypointsPerPhase.rawValue().toUInt(&value));
+    Q_ASSERT(value);
+    (void)value;
+
+    if ( !_currentManager->update() ) {
+        Q_ASSERT(false);
+    }
+
+    emit missionItemsChanged();
+    emit currentMissionItemsChanged();
+    emit currentWaypointPathChanged();
+    emit waypointPathChanged();
+}
+
+void WimaController::_updateflightSpeed()
+{
+    bool value;
+    _managerSettings.setFlightSpeed(_flightSpeed.rawValue().toDouble(&value));
+    Q_ASSERT(value);
+    (void)value;
+
+    if ( !_currentManager->update() ) {
+        Q_ASSERT(false);
+    }
+
+    emit missionItemsChanged();
+    emit currentMissionItemsChanged();
+    emit currentWaypointPathChanged();
+    emit waypointPathChanged();
+}
+
+void WimaController::_updateArrivalReturnSpeed()
+{
+    bool value;
+    _managerSettings.setArrivalReturnSpeed(_arrivalReturnSpeed.rawValue().toDouble(&value));
+    Q_ASSERT(value);
+    (void)value;
+
+    if ( !_currentManager->update() ) {
+        Q_ASSERT(false);
+    }
+
+    emit missionItemsChanged();
+    emit currentMissionItemsChanged();
+    emit currentWaypointPathChanged();
+    emit waypointPathChanged();
+}
+
+void WimaController::_updateAltitude()
+{
+    bool value;
+    _managerSettings.setAltitude(_altitude.rawValue().toDouble(&value));
+    Q_ASSERT(value);
+    (void)value;
+
+    if ( !_currentManager->update() ) {
+        Q_ASSERT(false);
+    }
+
+    emit missionItemsChanged();
+    emit currentMissionItemsChanged();
+    emit currentWaypointPathChanged();
+    emit waypointPathChanged();
+}
+
+void WimaController::_checkBatteryLevel()
+{
+    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();
+
+    if (managerVehicle != nullptr && enabled == true) {
+        Fact *battery1percentRemaining = managerVehicle->battery1FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName);
+        Fact *battery2percentRemaining = managerVehicle->battery2FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName);
+
+
+        if (battery1percentRemaining->rawValue().toDouble() < batteryThreshold
+                && battery2percentRemaining->rawValue().toDouble() < batteryThreshold) {
+            if (!_lowBatteryHandlingTriggered) {                
+                _lowBatteryHandlingTriggered = true;
+                if ( !(_missionController->remainingTime() <= minTime) ) {
+                    requestSmartRTL();
+                }
+            }
+        }
+        else {
+            _lowBatteryHandlingTriggered = false;
+        }
+
+    }
+}
+
+void WimaController::_eventTimerHandler()
+{
+    static EventTicker batteryLevelTicker(EVENT_TIMER_INTERVAL, CHECK_BATTERY_INTERVAL);
+    static EventTicker snakeTicker(EVENT_TIMER_INTERVAL, SNAKE_EVENT_LOOP_INTERVAL);
+    static EventTicker nemoStatusTicker(EVENT_TIMER_INTERVAL, 5000);
+
+    // Battery level check necessary?
+    Fact *enableLowBatteryHandling = qgcApp()->toolbox()->settingsManager()->wimaSettings()->enableLowBatteryHandling();
+    if ( enableLowBatteryHandling->rawValue().toBool() && batteryLevelTicker.ready() )
+        _checkBatteryLevel();
+
+    // Snake flight plan update necessary?
+//    if ( snakeEventLoopTicker.ready() ) {
+//        if ( _enableSnake.rawValue().toBool() && _localPlanDataValid && !_snakeCalcInProgress && _scenarioDefinedBool) {
+//        }
+//    }
+
+    if ( nemoStatusTicker.ready() ) {
+        this->_nemoHeartbeat.setStatus(_fallbackStatus);
+        emit WimaController::nemoStatusChanged();
+        emit WimaController::nemoStatusStringChanged();
+    }
+
+    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;
+        }
+    }
+}
+
+void WimaController::_smartRTLCleanUp(bool flying)
+{
+    if ( !flying) { // vehicle has landed
+        _switchWaypointManager(_defaultManager);
+        _missionController->removeAllFromVehicle();
+        _missionController->removeAll();
+        disconnect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp);
+    }
+}
+
+void WimaController::_setPhaseDistance(double distance)
+{
+    (void)distance;
+//    if (!qFuzzyCompare(distance, _phaseDistance)) {
+//        _phaseDistance = distance;
+
+//        emit phaseDistanceChanged();
+//    }
+}
+
+void WimaController::_setPhaseDuration(double duration)
+{
+    (void)duration;
+//    if (!qFuzzyCompare(duration, _phaseDuration)) {
+//        _phaseDuration = duration;
+
+//        emit phaseDurationChanged();
+//    }
+}
+
+bool WimaController::_checkSmartRTLPreCondition(QString &errorString)
+{
+    if (!_localPlanDataValid) {
+        errorString.append(tr("No WiMA data available. Please define at least a measurement and a service area."));
+        return false;
+    }
+
+    return _rtlManager.checkPrecondition(errorString);
+}
+
+void WimaController::_switchWaypointManager(WaypointManager::ManagerBase &manager)
+{
+    if (_currentManager != &manager) {
+        _currentManager = &manager;
+
+        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);
+
+        emit missionItemsChanged();
+        emit currentMissionItemsChanged();
+        emit waypointPathChanged();
+        emit currentWaypointPathChanged();
+
+        qWarning() << "WimaController::_switchWaypointManager: statistics update missing.";
+    }
+}
+
+void WimaController::_initSmartRTL()
+{
+    QString errorString;
+    static int attemptCounter = 0;
+    attemptCounter++;
+
+    if ( _checkSmartRTLPreCondition(errorString) ) {
+        _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);
+    }
+}
+
+void WimaController::_setSnakeCalcInProgress(bool inProgress)
+{
+    if (_snakeCalcInProgress != inProgress){
+        _snakeCalcInProgress = inProgress;
+        emit snakeCalcInProgressChanged();
+    }
+}
+
+void WimaController::_updateSnakeData()
+{    
+    _setSnakeCalcInProgress(false);
+    auto start = std::chrono::high_resolution_clock::now();
+    _snakeManager.clear();
+    const auto& r = _snakeWorker.result();
+    if (!r.success) {
+        //qgcApp()->showMessage(r.errorMessage);
+        return;
+    }
+
+    // create Mission items from r.waypoints
+    long n = r.waypoints.size() - r.returnPathIdx.size() - r.arrivalPathIdx.size() + 2;
+    if (n < 1) {
+        return;
+    }
+
+    // Create QVector<QGeoCoordinate> containing all waypoints;
+    unsigned long startIdx = r.arrivalPathIdx.last();
+    unsigned  long endIdx = r.returnPathIdx.first();
+    for (unsigned long i = startIdx; i <= endIdx; ++i) {
+        _snakeManager.push_back(r.waypoints[int(i)]);
+    }    
+    auto end = std::chrono::high_resolution_clock::now();
+    double duration = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
+    qWarning() << "WimaController: push_back waypoints execution time: " << duration << " ms.";
+
+
+    start = std::chrono::high_resolution_clock::now();
+    _snakeManager.update();
+    end         = std::chrono::high_resolution_clock::now();
+    duration    = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
+    qWarning() << "WimaController: _snakeManager.update(); execution time: " << duration << " ms.";
+
+
+    start = std::chrono::high_resolution_clock::now();
+    emit missionItemsChanged();
+    emit currentMissionItemsChanged();
+    emit currentWaypointPathChanged();
+    emit waypointPathChanged();
+    end         = std::chrono::high_resolution_clock::now();
+    duration    = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
+    qWarning() << "WimaController: gui update execution time: " << duration << " ms.";
+
+    start = std::chrono::high_resolution_clock::now();
+    _snakeOrigin            = r.origin;
+    _snakeTileCenterPoints  = r.tileCenterPoints;
+    _snakeTiles             = r.tiles;
+    _snakeTilesLocal        = r.tilesLocal;
+    end         = std::chrono::high_resolution_clock::now();
+    duration    = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
+    qWarning() << "WimaController: tiles copy execution time: " << duration << " ms.";
+
+    start = std::chrono::high_resolution_clock::now();
+    emit snakeTilesChanged();
+    emit snakeTileCenterPointsChanged();
+    end         = std::chrono::high_resolution_clock::now();
+    duration    = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
+    qWarning() << "WimaController: gui update 2 execution time: " << duration << " ms.";
+}
+
+void WimaController::_initStartSnakeWorker()
+{
+    if ( !_enableSnake.rawValue().toBool() )
+        return;
+
+    // Stop worker thread if running.
+    if ( _snakeWorker.isRunning() ) {
+        _snakeWorker.quit();
+    }
+
+    // Initialize _snakeWorker.
+    _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());
+    _setSnakeCalcInProgress(true);
+
+    // Start worker thread.
+    _snakeWorker.start();
+}
+
+void WimaController::_switchSnakeManager(QVariant variant)
+{
+    if (variant.value<bool>()){
+        _switchWaypointManager(_snakeManager);
+    } else {
+        _switchWaypointManager(_defaultManager);
+    }
+}

src/Wima/WimaController_new.h

+#pragma once
+
+#include <QObject>
+#include <QScopedPointer>
+
+#include "QGCMapPolygon.h"
+#include "QmlObjectListModel.h"
+
+#include "Geometry/WimaArea.h"
+#include "Geometry/WimaMeasurementArea.h"
+#include "Geometry/WimaServiceArea.h"
+#include "Geometry/WimaCorridor.h"
+#include "Geometry/WimaMeasurementAreaData.h"
+#include "Geometry/WimaCorridorData.h"
+#include "Geometry/WimaServiceAreaData.h"
+
+#include "WimaPlanData.h"
+
+#include "PlanMasterController.h"
+#include "MissionController.h"
+#include "SurveyComplexItem.h"
+#include "SimpleMissionItem.h"
+#include "MissionSettingsItem.h"
+#include "JsonHelper.h"
+#include "QGCApplication.h"
+#include "SettingsFact.h"
+#include "WimaSettings.h"
+#include "SettingsManager.h"
+
+#include "snake.h"
+#include "Snake/SnakeWorker.h"
+#include "Snake/GeoPolygonArray.h"
+#include "Snake/PolygonArray.h"
+#include "Geometry/GeoPoint3D.h"
+#include "Snake/QNemoProgress.h"
+#include "Snake/QNemoHeartbeat.h"
+
+#include "ros_bridge/include/ROSBridge.h"
+
+#include "WaypointManager/DefaultManager.h"
+#include "WaypointManager/RTLManager.h"
+
+#include <map>
+
+#define CHECK_BATTERY_INTERVAL 1000 // ms
+#define SMART_RTL_MAX_ATTEMPTS 3 // times
+#define SMART_RTL_ATTEMPT_INTERVAL 200 // ms
+#define EVENT_TIMER_INTERVAL 50 // ms
+#define SNAKE_EVENT_LOOP_INTERVAL 1000 // ms
+
+
+using namespace snake;
+
+typedef std::unique_ptr<rapidjson::Document> JsonDocUPtr;
+
+class WimaController : public QObject
+{
+    Q_OBJECT
+
+    enum FileType {WimaFile, PlanFile};
+
+    typedef QScopedPointer<ROSBridge::ROSBridge> ROSBridgePtr;
+
+public:
+
+
+    WimaController(QObject *parent = nullptr);
+
+    // Controllers.
+    Q_PROPERTY(PlanMasterController*    masterController
+               READ                     masterController
+               WRITE                    setMasterController
+               NOTIFY                   masterControllerChanged
+               )
+    Q_PROPERTY(MissionController*       missionController
+               READ                     missionController
+               WRITE                    setMissionController
+               NOTIFY                   missionControllerChanged
+               )
+    // Wima Data.
+    Q_PROPERTY(QmlObjectListModel*      visualItems
+               READ                     visualItems
+               NOTIFY                   visualItemsChanged
+               )
+    Q_PROPERTY(QmlObjectListModel*  missionItems
+               READ                 missionItems
+               NOTIFY               missionItemsChanged
+               )
+    Q_PROPERTY(QmlObjectListModel*  currentMissionItems
+               READ                 currentMissionItems
+               NOTIFY               currentMissionItemsChanged
+               )
+    Q_PROPERTY(QVariantList waypointPath
+               READ         waypointPath
+               NOTIFY       waypointPathChanged
+               )
+    Q_PROPERTY(QVariantList currentWaypointPath
+               READ         currentWaypointPath
+               NOTIFY       currentWaypointPathChanged
+               )
+    Q_PROPERTY(Fact*                    enableWimaController
+               READ enableWimaController
+               CONSTANT)
+    // Waypoint navigaton.
+    Q_PROPERTY(Fact*    overlapWaypoints
+               READ     overlapWaypoints
+               CONSTANT
+               )
+    Q_PROPERTY(Fact* maxWaypointsPerPhase
+               READ  maxWaypointsPerPhase
+               CONSTANT
+               )
+    Q_PROPERTY(Fact*    startWaypointIndex
+               READ     startWaypointIndex
+               CONSTANT
+               )
+    Q_PROPERTY(Fact*    showAllMissionItems
+               READ     showAllMissionItems
+               CONSTANT
+               )
+    Q_PROPERTY(Fact*    showCurrentMissionItems
+               READ     showCurrentMissionItems
+               CONSTANT
+               )
+    // Waypoint settings.
+    Q_PROPERTY(Fact*    flightSpeed
+               READ     flightSpeed
+               CONSTANT
+               )
+    Q_PROPERTY(Fact*    altitude
+               READ     altitude
+               CONSTANT
+               )
+    Q_PROPERTY(Fact*    arrivalReturnSpeed
+               READ     arrivalReturnSpeed
+               CONSTANT
+               )
+    // Waypoint statistics.
+    Q_PROPERTY(double   phaseDistance
+               READ     phaseDistance
+               NOTIFY   phaseDistanceChanged
+               )
+    Q_PROPERTY(double   phaseDuration
+               READ     phaseDuration
+               NOTIFY   phaseDurationChanged
+               )
+
+    // Snake
+    Q_PROPERTY(Fact*    enableSnake
+               READ     enableSnake
+               CONSTANT
+               )
+    Q_PROPERTY(int      nemoStatus
+               READ     nemoStatus
+               NOTIFY   nemoStatusChanged
+               )
+    Q_PROPERTY(QString  nemoStatusString
+               READ     nemoStatusString
+               NOTIFY   nemoStatusStringChanged
+               )
+    Q_PROPERTY(bool     snakeCalcInProgress
+               READ     snakeCalcInProgress
+               NOTIFY   snakeCalcInProgressChanged
+               )
+    Q_PROPERTY(Fact*    snakeTileWidth
+               READ     snakeTileWidth
+               CONSTANT
+               )
+    Q_PROPERTY(Fact*    snakeTileHeight
+               READ     snakeTileHeight
+               CONSTANT
+               )
+    Q_PROPERTY(Fact*    snakeMinTileArea
+               READ     snakeMinTileArea
+               CONSTANT
+               )
+    Q_PROPERTY(Fact*    snakeLineDistance
+               READ     snakeLineDistance
+               CONSTANT
+               )
+    Q_PROPERTY(Fact*    snakeMinTransectLength
+               READ     snakeMinTransectLength
+               CONSTANT
+               )
+    Q_PROPERTY(QmlObjectListModel*  snakeTiles
+               READ                 snakeTiles
+               NOTIFY               snakeTilesChanged
+               )
+    Q_PROPERTY(QVariantList snakeTileCenterPoints
+               READ         snakeTileCenterPoints
+               NOTIFY       snakeTileCenterPointsChanged
+               )
+    Q_PROPERTY(QVector<int>     nemoProgress
+               READ             nemoProgress
+               NOTIFY           nemoProgressChanged
+               )
+
+
+
+    // Property accessors
+    // Controllers.
+    PlanMasterController*       masterController       (void);
+    MissionController*          missionController      (void);
+    // Wima Data
+    QmlObjectListModel*         visualItems            (void);
+    QGCMapPolygon               joinedArea             (void) const;
+    // Waypoints.
+    QmlObjectListModel*         missionItems           (void);
+    QmlObjectListModel*         currentMissionItems    (void);
+    QVariantList                waypointPath           (void);
+    QVariantList                currentWaypointPath    (void);
+    // Settings facts.
+    Fact*                       enableWimaController   (void);
+    Fact*                       overlapWaypoints       (void);
+    Fact*                       maxWaypointsPerPhase   (void);
+    Fact*                       startWaypointIndex     (void);
+    Fact*                       showAllMissionItems    (void);
+    Fact*                       showCurrentMissionItems(void);
+    Fact*                       flightSpeed            (void);
+    Fact*                       arrivalReturnSpeed     (void);
+    Fact*                       altitude               (void);
+    // Snake settings facts.
+    Fact*                       enableSnake            (void)           { return &_enableSnake; }
+    Fact*                       snakeTileWidth         (void)           { return &_snakeTileWidth;}
+    Fact*                       snakeTileHeight        (void)           { return &_snakeTileHeight;}
+    Fact*                       snakeMinTileArea       (void)           { return &_snakeMinTileArea;}
+    Fact*                       snakeLineDistance      (void)           { return &_snakeLineDistance;}
+    Fact*                       snakeMinTransectLength (void)           { return &_snakeMinTransectLength;}
+    // Snake data.
+    QmlObjectListModel*         snakeTiles             (void)           { return _snakeTiles.QmlObjectListModel();}
+    QVariantList                snakeTileCenterPoints  (void)           { return _snakeTileCenterPoints;}
+    QVector<int>                nemoProgress           (void);
+    int                         nemoStatus             (void) const;
+    QString                     nemoStatusString       (void) const;
+    bool                        snakeCalcInProgress    (void) const;
+
+    // Smart RTL.
+    bool                        uploadOverrideRequired (void) const;
+    bool                        vehicleHasLowBattery   (void) const;
+    // Waypoint statistics.
+    double                      phaseDistance          (void) const;
+    double                      phaseDuration          (void) const;
+
+
+    // Property setters
+    void setMasterController        (PlanMasterController* masterController);
+    void setMissionController       (MissionController* missionController);
+    bool setWimaPlanData            (const WimaPlanData &planData);
+
+    // Member Methodes
+    Q_INVOKABLE WimaController *thisPointer();
+    // Waypoint navigation.
+    Q_INVOKABLE void nextPhase();
+    Q_INVOKABLE void previousPhase();
+    Q_INVOKABLE void resetPhase();
+    // Smart RTL.
+    Q_INVOKABLE void requestSmartRTL();
+    Q_INVOKABLE void initSmartRTL();
+    Q_INVOKABLE void executeSmartRTL();
+    // Other.
+    Q_INVOKABLE void removeVehicleTrajectoryHistory();
+    Q_INVOKABLE bool upload();
+    Q_INVOKABLE bool forceUpload();
+    Q_INVOKABLE void removeFromVehicle();
+
+
+    // static Members
+    static const char* areaItemsName;
+    static const char* missionItemsName;    
+    static const char* settingsGroup;
+    static const char* endWaypointIndexName;
+    static const char* enableWimaControllerName;
+    static const char* overlapWaypointsName;
+    static const char* maxWaypointsPerPhaseName;
+    static const char* startWaypointIndexName;
+    static const char* showAllMissionItemsName;
+    static const char* showCurrentMissionItemsName;
+    static const char* flightSpeedName;
+    static const char* arrivalReturnSpeedName;
+    static const char* altitudeName;
+    static const char* snakeTileWidthName;
+    static const char* snakeTileHeightName;
+    static const char* snakeMinTileAreaName;
+    static const char* snakeLineDistanceName;
+    static const char* snakeMinTransectLengthName;
+
+signals:
+    // Controllers.
+    void masterControllerChanged            (void);
+    void missionControllerChanged           (void);
+    // Wima data.
+    void visualItemsChanged                 (void);
+    // Waypoints.
+    void missionItemsChanged                (void);
+    void currentMissionItemsChanged         (void);
+    void waypointPathChanged                (void);
+    void currentWaypointPathChanged         (void);
+    // Smart RTL.
+    void smartRTLRequestConfirm             (void);
+    void smartRTLPathConfirm                (void);
+    // Upload.
+    void forceUploadConfirm                 (void);
+    // Waypoint statistics.
+    void phaseDistanceChanged               (void);
+    void phaseDurationChanged               (void);
+    // Snake.
+    void snakeConnectionStatusChanged       (void);
+    void snakeCalcInProgressChanged         (void);
+    void snakeTilesChanged                  (void);
+    void snakeTileCenterPointsChanged       (void);
+    void nemoProgressChanged                (void);
+    void nemoStatusChanged                  (void);
+    void nemoStatusStringChanged            (void);
+
+private slots:
+
+    // Waypoint navigation / helper.
+    bool _calcNextPhase         (void);
+    void _recalcCurrentPhase    (void);
+    bool _calcShortestPath      (const QGeoCoordinate &start,
+                                 const QGeoCoordinate &destination,
+                                 QVector<QGeoCoordinate> &path);
+    // Slicing parameters
+    bool _setStartIndex             (void);
+    void _updateOverlap             (void);
+    void _updateMaxWaypoints        (void);
+    // Waypoint settings.
+    void _updateflightSpeed         (void);
+    void _updateArrivalReturnSpeed  (void);
+    void _updateAltitude            (void);
+    // Waypoint Statistics.
+    void _setPhaseDistance  (double distance);
+    void _setPhaseDuration  (double duration);
+    // SMART RTL
+    void    _checkBatteryLevel                  (void);
+    bool    _checkSmartRTLPreCondition          (QString &errorString);
+    void    _initSmartRTL                       ();
+    void    _smartRTLCleanUp                    (bool flying);
+    // Snake.
+    void _setSnakeCalcInProgress             (bool inProgress);
+    void _updateSnakeData                    ();
+    void _initStartSnakeWorker               ();
+    void _switchSnakeManager                 (QVariant variant);
+    // Periodic tasks.
+    void _eventTimerHandler                  (void);
+    // Waypoint manager handling.
+    void _switchWaypointManager(WaypointManager::ManagerBase &manager);
+
+private:
+    using StatusMap = std::map<int, QString>;
+
+    // Controllers.
+    PlanMasterController   *_masterController;
+    MissionController      *_missionController;
+
+    // Wima Data.
+    QmlObjectListModel      _areas; // contains all visible areas
+    WimaJoinedAreaData      _joinedArea; // joined area fromed by opArea, serArea, _corridor
+    WimaMeasurementAreaData _measurementArea; // measurement area
+    WimaServiceAreaData     _serviceArea; // area for supplying
+    WimaCorridorData        _corridor; // corridor connecting opArea and serArea
+    bool                    _localPlanDataValid;
+
+    // Waypoint Managers.
+    WaypointManager::AreaInterface      _areaInterface;
+    WaypointManager::Settings           _managerSettings;
+    WaypointManager::DefaultManager     _defaultManager;
+    WaypointManager::DefaultManager     _snakeManager;
+    WaypointManager::RTLManager         _rtlManager;
+    WaypointManager::ManagerBase       *_currentManager;
+    using ManagerList = QList<WaypointManager::ManagerBase*>;
+    ManagerList                         _managerList;
+
+    // Settings Facts.
+    QMap<QString, FactMetaData*> _metaDataMap;
+    SettingsFact                 _enableWimaController; // enables or disables the wimaControler
+    SettingsFact                 _overlapWaypoints; // determines the number of overlapping waypoints between two consecutive mission phases
+    SettingsFact                 _maxWaypointsPerPhase; // determines the maximum number waypoints per phase
+    SettingsFact                 _nextPhaseStartWaypointIndex; // index (displayed on the map, -1 to get index of item in _missionItems) of the mission item
+                                                      // defining the first element of the next phase
+    SettingsFact                 _showAllMissionItems; // bool value, Determines whether the mission items of the overall mission are displayed or not.
+    SettingsFact                 _showCurrentMissionItems; // bool value, Determines whether the mission items of the current mission phase are displayed or not.
+    SettingsFact                 _flightSpeed; // mission flight speed
+    SettingsFact                 _arrivalReturnSpeed; // arrival and return path speed
+    SettingsFact                 _altitude; // mission altitude
+    SettingsFact                 _enableSnake; // Enable Snake (see snake.h)
+    SettingsFact                 _snakeTileWidth;
+    SettingsFact                 _snakeTileHeight;
+    SettingsFact                 _snakeMinTileArea;
+    SettingsFact                 _snakeLineDistance;
+    SettingsFact                 _snakeMinTransectLength;
+
+    // Smart RTL.
+    QTimer          _smartRTLTimer;
+    bool            _lowBatteryHandlingTriggered;
+
+    // Waypoint statistics.
+    double                       _measurementPathLength; // the lenght of the phase in meters
+//    double                       _phaseDistance; // the lenth in meters of the current phase
+//    double                       _phaseDuration; // the phase duration in seconds
+
+    // Snake
+    bool                    _snakeCalcInProgress;
+    SnakeDataManager             _snakeWorker;
+    GeoPoint                _snakeOrigin;
+    GeoPolygonArray         _snakeTiles; // tiles
+    PolygonArray            _snakeTilesLocal; // tiles local coordinate system
+    QVariantList            _snakeTileCenterPoints;
+    QNemoProgress           _nemoProgress; // measurement progress
+    QNemoHeartbeat          _nemoHeartbeat; // measurement progress
+    int                     _fallbackStatus;
+    ROSBridgePtr            _pRosBridge;
+    bool                    _bridgeSetupDone;
+    static StatusMap        _nemoStatusMap;
+
+    // Periodic tasks.
+    QTimer                  _eventTimer;
+
+};
+
+

src/comm/ros_bridge/include/RosBridgeClient.h

@@ -10,101 +10,98 @@
 #include "rapidjson/include/rapidjson/document.h"
 #include "rapidjson/include/rapidjson/writer.h"
 #include "rapidjson/include/rapidjson/stringbuffer.h"
-#include "rapidjson/include/rapidjson/ostreamwrapper.h"
 
 #include <chrono>
 #include <functional>
 #include <thread>
 #include <future>
-#include <functional>
+#include <mutex>
 
 using WsClient = SimpleWeb::SocketClient<SimpleWeb::WS>;
 using InMessage =  std::function<void(std::shared_ptr<WsClient::Connection>, std::shared_ptr<WsClient::InMessage>)>;
 
 class RosbridgeWsClient
 {
- std::string server_port_path;
- std::unordered_map<std::string, std::shared_ptr<WsClient>> client_map;
-
- // Starts the client.
- void start(const std::string& client_name, std::shared_ptr<WsClient> client, const std::string& message)
- {
-   if (!client->on_open)
-   {
-#ifdef ROS_BRIDGE_CLIENT_DEBUG
-     client->on_open = [client_name, message](std::shared_ptr<WsClient::Connection> connection) {
+  std::string server_port_path;
+  std::unordered_map<std::string, std::shared_ptr<WsClient>> client_map;
+  std::mutex map_mutex;
+
+  void start(const std::string& client_name, std::shared_ptr<WsClient> client, const std::string& message)
+  {
+    if (!client->on_open)
+    {
+#ifdef DEBUG
+      client->on_open = [client_name, message](std::shared_ptr<WsClient::Connection> connection) {
 #else
-     client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
+      client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
 #endif
 
-#ifdef ROS_BRIDGE_CLIENT_DEBUG
-       std::cout << client_name << ": Opened connection" << std::endl;
-       std::cout << client_name << ": Sending message: " << message << std::endl;
+#ifdef DEBUG
+        std::cout << client_name << ": Opened connection" << std::endl;
+        std::cout << client_name << ": Sending message: " << message << std::endl;
 #endif
-       connection->send(message);
-     };
-   }
-
-#ifdef ROS_BRIDGE_CLIENT_DEBUG
-   if (!client->on_message)
-   {
-     client->on_message = [client_name](std::shared_ptr<WsClient::Connection> /*connection*/, std::shared_ptr<WsClient::InMessage> in_message) {
-       std::cout << client_name << ": Message received: " << in_message->string() << std::endl;
-     };
-   }
-
-   if (!client->on_close)
-   {
-     client->on_close = [client_name](std::shared_ptr<WsClient::Connection> /*connection*/, int status, const std::string & /*reason*/) {
-       std::cout << client_name << ": Closed connection with status code " << status << std::endl;
-     };
-   }
-
-   if (!client->on_error)
-   {
-     // See http://www.boost.org/doc/libs/1_55_0/doc/html/boost_asio/reference.html, Error Codes for error code meanings
-     client->on_error = [client_name](std::shared_ptr<WsClient::Connection> /*connection*/, const SimpleWeb::error_code &ec) {
-       std::cout << client_name << ": Error: " << ec << ", error message: " << ec.message() << std::endl;
-     };
-   }
+        connection->send(message);
+      };
+    }
+
+#ifdef DEBUG
+    if (!client->on_message)
+    {
+      client->on_message = [client_name](std::shared_ptr<WsClient::Connection> /*connection*/, std::shared_ptr<WsClient::InMessage> in_message) {
+        std::cout << client_name << ": Message received: " << in_message->string() << std::endl;
+      };
+    }
+
+    if (!client->on_close)
+    {
+      client->on_close = [client_name](std::shared_ptr<WsClient::Connection> /*connection*/, int status, const std::string & /*reason*/) {
+        std::cout << client_name << ": Closed connection with status code " << status << std::endl;
+      };
+    }
+
+    if (!client->on_error)
+    {
+      // See http://www.boost.org/doc/libs/1_55_0/doc/html/boost_asio/reference.html, Error Codes for error code meanings
+      client->on_error = [client_name](std::shared_ptr<WsClient::Connection> /*connection*/, const SimpleWeb::error_code &ec) {
+        std::cout << client_name << ": Error: " << ec << ", error message: " << ec.message() << std::endl;
+      };
+    }
 #endif
 
-#ifdef ROS_BRIDGE_CLIENT_DEBUG
-   std::thread client_thread([client_name, client]() {
+#ifdef DEBUG
+    std::thread client_thread([client_name, client]() {
 #else
-   std::thread client_thread([client]() {
+    std::thread client_thread([client]() {
 #endif
-     client->start();
+      client->start();
 
-#ifdef ROS_BRIDGE_CLIENT_DEBUG
-     std::cout << client_name << ": Terminated" << std::endl;
+#ifdef DEBUG
+      std::cout << client_name << ": Terminated" << std::endl;
 #endif
-     client->on_open = NULL;
-     client->on_message = NULL;
-     client->on_close = NULL;
-     client->on_error = NULL;
-   });
-
-   client_thread.detach();
+      client->on_open = NULL;
+      client->on_message = NULL;
+      client->on_close = NULL;
+      client->on_error = NULL;
+    });
 
-   // This is to make sure that the thread got fully launched before we do anything to it (e.g. remove)
-   std::this_thread::sleep_for(std::chrono::milliseconds(100));
- }
+    client_thread.detach();
+  }
 
 public:
- RosbridgeWsClient(const std::string& server_port_path)
- {
-   this->server_port_path = server_port_path;
- }
-
- ~RosbridgeWsClient()
- {
-   for (auto& client : client_map)
-   {
-     client.second->stop();
-     client.second.reset();
-   }
- }
+  RosbridgeWsClient(const std::string& server_port_path)
+  {
+    this->server_port_path = server_port_path;
+  }
+
+  ~RosbridgeWsClient()
+  {
+    std::lock_guard<std::mutex> lk(map_mutex); // neccessary?
+    for (auto& client : client_map)
+    {
+      client.second->stop();
+      client.second.reset();
+    }
+  }
 
  // The execution can take up to 100 ms!
  bool connected(){
@@ -133,267 +130,386 @@ public:
     return status == std::future_status::ready;
  }
 
- // Adds a client to the client_map.
- void addClient(const std::string& client_name)
- {
-   std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
-   if (it == client_map.end())
-   {
-     client_map[client_name] = std::make_shared<WsClient>(server_port_path);
-   }
-#ifdef ROS_BRIDGE_CLIENT_DEBUG
-   else
-   {
-     std::cerr << client_name << " has already been created" << std::endl;
-   }
+  // Adds a client to the client_map.
+  void addClient(const std::string& client_name)
+  {
+    std::lock_guard<std::mutex> lk(map_mutex);
+    std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
+    if (it == client_map.end())
+    {
+      client_map[client_name] = std::make_shared<WsClient>(server_port_path);
+    }
+#ifdef DEBUG
+    else
+    {
+      std::cerr << client_name << " has already been created" << std::endl;
+    }
 #endif
- }
-
- std::shared_ptr<WsClient> getClient(const std::string& client_name)
- {
-   std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
-   if (it != client_map.end())
-   {
-     return it->second;
-   }
-   return NULL;
- }
-
- void stopClient(const std::string& client_name)
- {
-   std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
-   if (it != client_map.end())
-   {
-     it->second->stop();
-     it->second->on_open = NULL;
-     it->second->on_message = NULL;
-     it->second->on_close = NULL;
-     it->second->on_error = NULL;
-#ifdef ROS_BRIDGE_CLIENT_DEBUG
-     std::cout << client_name << " has been stopped" << std::endl;
+  }
+
+  std::shared_ptr<WsClient> getClient(const std::string& client_name)
+  {
+    std::lock_guard<std::mutex> lk(map_mutex);
+    std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
+    if (it != client_map.end())
+    {
+      return it->second;
+    }
+    return NULL;
+  }
+
+  void stopClient(const std::string& client_name)
+  {
+    std::lock_guard<std::mutex> lk(map_mutex);
+    std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
+    if (it != client_map.end())
+    {
+      it->second->stop();
+      it->second->on_open = NULL;
+      it->second->on_message = NULL;
+      it->second->on_close = NULL;
+      it->second->on_error = NULL;
+#ifdef DEBUG
+      std::cout << client_name << " has been stopped" << std::endl;
 #endif
-   }
-#ifdef ROS_BRIDGE_CLIENT_DEBUG
-   else
-   {
-     std::cerr << client_name << " has not been created" << std::endl;
-   }
+    }
+#ifdef DEBUG
+    else
+    {
+      std::cerr << client_name << " has not been created" << std::endl;
+    }
 #endif
- }
-
- void removeClient(const std::string& client_name)
- {
-   std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
-   if (it != client_map.end())
-   {
-     it->second->stop();
-     it->second.reset();
-     client_map.erase(it);
-#ifdef ROS_BRIDGE_CLIENT_DEBUG
-     std::cout << client_name << " has been removed" << std::endl;
+  }
+
+  void removeClient(const std::string& client_name)
+  {
+    std::lock_guard<std::mutex> lk(map_mutex);
+    std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
+    if (it != client_map.end())
+    {
+      // Stop the client asynchronously in 100 ms.
+      // This is to ensure, that all threads involving the client have been launched.
+      std::thread t([](std::shared_ptr<WsClient> client){
+         std::this_thread::sleep_for(std::chrono::milliseconds(100));
+         client->stop();
+         client.reset();
+      },
+      it->second /*lambda param*/ );
+      client_map.erase(it);
+      t.detach();
+#ifdef DEBUG
+      std::cout << client_name << " has been removed" << std::endl;
 #endif
-   }
-#ifdef ROS_BRIDGE_CLIENT_DEBUG
-   else
-   {
-     std::cerr << client_name << " has not been created" << std::endl;
-   }
+    }
+#ifdef DEBUG
+    else
+    {
+      std::cerr << client_name << " has not been created" << std::endl;
+    }
 #endif
- }
-
- // Gets the client from client_map and starts it. Advertising is essentially sending a message.
- // One client per topic!
- void advertise(const std::string& client_name, const std::string& topic, const std::string& type, const std::string& id = "")
- {
-   std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
-   if (it != client_map.end())
-   {
-     std::string message = "\"op\":\"advertise\", \"topic\":\"" + topic + "\", \"type\":\"" + type + "\"";
-
-     if (id.compare("") != 0)
-     {
-       message += ", \"id\":\"" + id + "\"";
-     }
-     message = "{" + message + "}";
-
-     start(client_name, it->second, message);
-   }
-#ifdef ROS_BRIDGE_CLIENT_DEBUG
-   else
-   {
-     std::cerr << client_name << "has not been created" << std::endl;
-   }
+  }
+
+  // Gets the client from client_map and starts it. Advertising is essentially sending a message.
+  // One client per topic!
+  void advertise(const std::string& client_name, const std::string& topic, const std::string& type, const std::string& id = "")
+  {
+    std::lock_guard<std::mutex> lk(map_mutex);
+    std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
+    if (it != client_map.end())
+    {
+      std::string message = "\"op\":\"advertise\", \"topic\":\"" + topic + "\", \"type\":\"" + type + "\"";
+
+      if (id.compare("") != 0)
+      {
+        message += ", \"id\":\"" + id + "\"";
+      }
+      message = "{" + message + "}";
+
+      start(client_name, it->second, message);
+    }
+#ifdef DEBUG
+    else
+    {
+      std::cerr << client_name << "has not been created" << std::endl;
+    }
 #endif
- }
+  }
 
- void publish(const std::string& topic, const rapidjson::Document& msg, const std::string& id = "")
- {
-   rapidjson::StringBuffer strbuf;
-   rapidjson::Writer<rapidjson::StringBuffer> writer(strbuf);
-   msg.Accept(writer);
+  void publish(const std::string& topic, const rapidjson::Document& msg, const std::string& id = "")
+  {
+    rapidjson::StringBuffer strbuf;
+    rapidjson::Writer<rapidjson::StringBuffer> writer(strbuf);
+    msg.Accept(writer);
 
-   std::string message = "\"op\":\"publish\", \"topic\":\"" + topic + "\", \"msg\":" + strbuf.GetString();
+    std::string message = "\"op\":\"publish\", \"topic\":\"" + topic + "\", \"msg\":" + strbuf.GetString();
 
-   if (id.compare("") != 0)
-   {
-     message += ", \"id\":\"" + id + "\"";
-   }
-   message = "{" + message + "}";
+    if (id.compare("") != 0)
+    {
+      message += ", \"id\":\"" + id + "\"";
+    }
+    message = "{" + message + "}";
 
-   std::shared_ptr<WsClient> publish_client = std::make_shared<WsClient>(server_port_path);
+    std::shared_ptr<WsClient> publish_client = std::make_shared<WsClient>(server_port_path);
 
-   publish_client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
-#ifdef ROS_BRIDGE_CLIENT_DEBUG
-     std::cout << "publish_client: Opened connection" << std::endl;
-     std::cout << "publish_client: Sending message: " << message << std::endl;
+    publish_client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
+#ifdef DEBUG
+      std::cout << "publish_client: Opened connection" << std::endl;
+      std::cout << "publish_client: Sending message: " << message << std::endl;
 #endif
-     connection->send(message);
+      connection->send(message);
 
-     // TODO: This could be improved by creating a thread to keep publishing the message instead of closing it right away
-     connection->send_close(1000);
-   };
-
-   start("publish_client", publish_client, message);
- }
+      // TODO: This could be improved by creating a thread to keep publishing the message instead of closing it right away
+      connection->send_close(1000);
+    };
 
- void subscribe(const std::string& client_name, const std::string& topic, const InMessage& callback, const std::string& id = "", const std::string& type = "", int throttle_rate = -1, int queue_length = -1, int fragment_size = -1, const std::string& compression = "")
- {
-   std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
-   if (it != client_map.end())
-   {
-     std::string message = "\"op\":\"subscribe\", \"topic\":\"" + topic + "\"";
-
-     if (id.compare("") != 0)
-     {
-       message += ", \"id\":\"" + id + "\"";
-     }
-     if (type.compare("") != 0)
-     {
-       message += ", \"type\":\"" + type + "\"";
-     }
-     if (throttle_rate > -1)
-     {
-       message += ", \"throttle_rate\":" + std::to_string(throttle_rate);
-     }
-     if (queue_length > -1)
-     {
-       message += ", \"queue_length\":" + std::to_string(queue_length);
-     }
-     if (fragment_size > -1)
-     {
-       message += ", \"fragment_size\":" + std::to_string(fragment_size);
-     }
-     if (compression.compare("none") == 0 || compression.compare("png") == 0)
-     {
-       message += ", \"compression\":\"" + compression + "\"";
-     }
-     message = "{" + message + "}";
-
-     it->second->on_message = callback;
-     start(client_name, it->second, message);
-   }
-#ifdef ROS_BRIDGE_CLIENT_DEBUG
-   else
-   {
-     std::cerr << client_name << "has not been created" << std::endl;
-   }
+    start("publish_client", publish_client, message);
+  }
+
+  void subscribe(const std::string& client_name, const std::string& topic, const InMessage& callback, const std::string& id = "", const std::string& type = "", int throttle_rate = -1, int queue_length = -1, int fragment_size = -1, const std::string& compression = "")
+  {
+    std::lock_guard<std::mutex> lk(map_mutex);
+    std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
+    if (it != client_map.end())
+    {
+      std::string message = "\"op\":\"subscribe\", \"topic\":\"" + topic + "\"";
+
+      if (id.compare("") != 0)
+      {
+        message += ", \"id\":\"" + id + "\"";
+      }
+      if (type.compare("") != 0)
+      {
+        message += ", \"type\":\"" + type + "\"";
+      }
+      if (throttle_rate > -1)
+      {
+        message += ", \"throttle_rate\":" + std::to_string(throttle_rate);
+      }
+      if (queue_length > -1)
+      {
+        message += ", \"queue_length\":" + std::to_string(queue_length);
+      }
+      if (fragment_size > -1)
+      {
+        message += ", \"fragment_size\":" + std::to_string(fragment_size);
+      }
+      if (compression.compare("none") == 0 || compression.compare("png") == 0)
+      {
+        message += ", \"compression\":\"" + compression + "\"";
+      }
+      message = "{" + message + "}";
+
+      it->second->on_message = callback;
+      start(client_name, it->second, message);
+    }
+#ifdef DEBUG
+    else
+    {
+      std::cerr << client_name << "has not been created" << std::endl;
+    }
 #endif
- }
-
- void advertiseService(const std::string& client_name, const std::string& service, const std::string& type, const InMessage& callback)
- {
-   std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
-   if (it != client_map.end())
-   {
-     std::string message = "{\"op\":\"advertise_service\", \"service\":\"" + service + "\", \"type\":\"" + type + "\"}";
-
-     it->second->on_message = callback;
-     start(client_name, it->second, message);
-   }
-#ifdef ROS_BRIDGE_CLIENT_DEBUG
-   else
-   {
-     std::cerr << client_name << "has not been created" << std::endl;
-   }
+  }
+
+  void advertiseService(const std::string& client_name, const std::string& service, const std::string& type, const InMessage& callback)
+  {
+    std::lock_guard<std::mutex> lk(map_mutex);
+    std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
+    if (it != client_map.end())
+    {
+      std::string message = "{\"op\":\"advertise_service\", \"service\":\"" + service + "\", \"type\":\"" + type + "\"}";
+
+      it->second->on_message = callback;
+      start(client_name, it->second, message);
+    }
+#ifdef DEBUG
+    else
+    {
+      std::cerr << client_name << "has not been created" << std::endl;
+    }
 #endif
- }
+  }
 
- void serviceResponse(const std::string& service, const std::string& id, bool result, const rapidjson::Document& values = {})
- {
-   std::string message = "\"op\":\"service_response\", \"service\":\"" + service + "\", \"result\":" + ((result)? "true" : "false");
+  void serviceResponse(const std::string& service, const std::string& id, bool result, const rapidjson::Document& values = {})
+  {
+    std::string message = "\"op\":\"service_response\", \"service\":\"" + service + "\", \"result\":" + ((result)? "true" : "false");
 
-   // Rosbridge somehow does not allow service_response to be sent without id and values
-   // , so we cannot omit them even though the documentation says they are optional.
-   message += ", \"id\":\"" + id + "\"";
+    // Rosbridge somehow does not allow service_response to be sent without id and values
+    // , so we cannot omit them even though the documentation says they are optional.
+    message += ", \"id\":\"" + id + "\"";
 
-   // Convert JSON document to string
-   rapidjson::StringBuffer strbuf;
-   rapidjson::Writer<rapidjson::StringBuffer> writer(strbuf);
-   values.Accept(writer);
+    // Convert JSON document to string
+    rapidjson::StringBuffer strbuf;
+    rapidjson::Writer<rapidjson::StringBuffer> writer(strbuf);
+    values.Accept(writer);
 
-   message += ", \"values\":" + std::string(strbuf.GetString());
-   message = "{" + message + "}";
+    message += ", \"values\":" + std::string(strbuf.GetString());
+    message = "{" + message + "}";
 
-   std::shared_ptr<WsClient> service_response_client = std::make_shared<WsClient>(server_port_path);
+    std::shared_ptr<WsClient> service_response_client = std::make_shared<WsClient>(server_port_path);
 
-   service_response_client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
-#ifdef ROS_BRIDGE_CLIENT_DEBUG
-     std::cout << "service_response_client: Opened connection" << std::endl;
-     std::cout << "service_response_client: Sending message: " << message << std::endl;
+    service_response_client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
+#ifdef DEBUG
+      std::cout << "service_response_client: Opened connection" << std::endl;
+      std::cout << "service_response_client: Sending message: " << message << std::endl;
 #endif
-     connection->send(message);
-
-     connection->send_close(1000);
-   };
+      connection->send(message);
 
-   start("service_response_client", service_response_client, message);
- }
+      connection->send_close(1000);
+    };
 
- void callService(const std::string& service, const InMessage& callback, const rapidjson::Document& args = {}, const std::string& id = "", int fragment_size = -1, const std::string& compression = "")
- {
-   std::string message = "\"op\":\"call_service\", \"service\":\"" + service + "\"";
-
-   if (!args.IsNull())
-   {
-     rapidjson::StringBuffer strbuf;
-     rapidjson::Writer<rapidjson::StringBuffer> writer(strbuf);
-     args.Accept(writer);
-
-     message += ", \"args\":" + std::string(strbuf.GetString());
-   }
-   if (id.compare("") != 0)
-   {
-     message += ", \"id\":\"" + id + "\"";
-   }
-   if (fragment_size > -1)
-   {
-     message += ", \"fragment_size\":" + std::to_string(fragment_size);
-   }
-   if (compression.compare("none") == 0 || compression.compare("png") == 0)
-   {
-     message += ", \"compression\":\"" + compression + "\"";
-   }
-   message = "{" + message + "}";
-
-   std::shared_ptr<WsClient> call_service_client = std::make_shared<WsClient>(server_port_path);
-
-   if (callback)
-   {
-     call_service_client->on_message = callback;
-   }
-   else
-   {
-     call_service_client->on_message = [](std::shared_ptr<WsClient::Connection> connection, std::shared_ptr<WsClient::InMessage> in_message) {
-#ifdef ROS_BRIDGE_CLIENT_DEBUG
-       std::cout << "call_service_client: Message received: " << in_message->string() << std::endl;
-       std::cout << "call_service_client: Sending close connection" << std::endl;
+    start("service_response_client", service_response_client, message);
+  }
+
+  void callService(const std::string& service, const InMessage& callback, const rapidjson::Document& args = {}, const std::string& id = "", int fragment_size = -1, const std::string& compression = "")
+  {
+    std::string message = "\"op\":\"call_service\", \"service\":\"" + service + "\"";
+
+    if (!args.IsNull())
+    {
+      rapidjson::StringBuffer strbuf;
+      rapidjson::Writer<rapidjson::StringBuffer> writer(strbuf);
+      args.Accept(writer);
+
+      message += ", \"args\":" + std::string(strbuf.GetString());
+    }
+    if (id.compare("") != 0)
+    {
+      message += ", \"id\":\"" + id + "\"";
+    }
+    if (fragment_size > -1)
+    {
+      message += ", \"fragment_size\":" + std::to_string(fragment_size);
+    }
+    if (compression.compare("none") == 0 || compression.compare("png") == 0)
+    {
+      message += ", \"compression\":\"" + compression + "\"";
+    }
+    message = "{" + message + "}";
+
+    std::shared_ptr<WsClient> call_service_client = std::make_shared<WsClient>(server_port_path);
+
+    if (callback)
+    {
+      call_service_client->on_message = callback;
+    }
+    else
+    {
+      call_service_client->on_message = [](std::shared_ptr<WsClient::Connection> connection, std::shared_ptr<WsClient::InMessage> in_message) {
+#ifdef DEBUG
+        std::cout << "call_service_client: Message received: " << in_message->string() << std::endl;
+        std::cout << "call_service_client: Sending close connection" << std::endl;
 #endif
-       connection->send_close(1000);
-     };
-   }
-
-   start("call_service_client", call_service_client, message);
- }
+        connection->send_close(1000);
+      };
+    }
+
+    start("call_service_client", call_service_client, message);
+  }
+
+  //!
+  //! \brief Checks if the service \p service is available.
+  //! \param service Service name.
+  //! \return Returns true if the service is available, false either.
+  //! \note Don't call this function to frequently. Use \fn waitForService() instead.
+  //!
+  bool serviceAvailable(const std::string& service)
+  {
+    const rapidjson::Document args = {};
+    const std::string& id = "";
+    const int fragment_size = -1;
+    const std::string& compression = "";
+    std::string message = "\"op\":\"call_service\", \"service\":\"" + service + "\"";
+    std::string client_name("wait_for_service_client");
+
+    if (!args.IsNull())
+    {
+      rapidjson::StringBuffer strbuf;
+      rapidjson::Writer<rapidjson::StringBuffer> writer(strbuf);
+      args.Accept(writer);
+
+      message += ", \"args\":" + std::string(strbuf.GetString());
+    }
+    if (id.compare("") != 0)
+    {
+      message += ", \"id\":\"" + id + "\"";
+    }
+    if (fragment_size > -1)
+    {
+      message += ", \"fragment_size\":" + std::to_string(fragment_size);
+    }
+    if (compression.compare("none") == 0 || compression.compare("png") == 0)
+    {
+      message += ", \"compression\":\"" + compression + "\"";
+    }
+    message = "{" + message + "}";
+
+    std::shared_ptr<WsClient> wait_for_service_client = std::make_shared<WsClient>(server_port_path);
+    std::shared_ptr<std::promise<bool>> p(std::make_shared<std::promise<bool>>());
+    auto future = p->get_future();
+#ifdef DEBUG
+    wait_for_service_client->on_message = [p, service, client_name](
+#else
+    wait_for_service_client->on_message = [p](
+#endif
+            std::shared_ptr<WsClient::Connection> connection,
+            std::shared_ptr<WsClient::InMessage> in_message){
+#ifdef DEBUG
+#endif
+        rapidjson::Document doc;
+        doc.Parse(in_message->string().c_str());
+        if ( !doc.HasParseError()
+             && doc.HasMember("result")
+             && doc["result"].IsBool()
+             && doc["result"] == true )
+        {
+#ifdef DEBUG
+            std::cout << client_name << ": "
+                      << "service " << service
+                      << " available." << std::endl;
+            std::cout << client_name << ": "
+                      << "message: " << in_message->string()
+                      << std::endl;
+#endif
+            p->set_value(true);
+        } else {
+            p->set_value(false);
+        }
+        connection->send_close(1000);
+    };
+    start(client_name, wait_for_service_client, message);
+    return future.get();
+  }
+
+  //!
+  //! \brief Waits until the service with the name \p service is available.
+  //! \param service Service name.
+  //! @note This function will block as long as the service is not available.
+  //!
+  void waitForService(const std::string& service)
+  {
+
+
+#ifdef DEBUG
+    auto s = std::chrono::high_resolution_clock::now();
+    long counter = 0;
+#endif
+    while(true)
+    {
+#ifdef DEBUG
+        ++counter;
+#endif
+        if (serviceAvailable(service)){
+            break;
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds(50)); // Avoid excessive polling.
+    };
+#ifdef DEBUG
+    auto e = std::chrono::high_resolution_clock::now();
+    std::cout << "waitForService(): time: "
+              << std::chrono::duration_cast<std::chrono::milliseconds>(e-s).count()
+              << " ms." << std::endl;
+    std::cout << "waitForService(): clients launched: "
+              << counter << std::endl;
+#endif
+  }
 };
-
-

src/comm/ros_bridge/include/Server.cpp

+#include "Server.h"
+
+Server::Server()
+{
+
+}

src/comm/ros_bridge/include/Server.h

+#ifndef SERVER_H
+#define SERVER_H
+
+
+class Server
+{
+public:
+    Server();
+};
+
+#endif // SERVER_H

src/comm/ros_bridge/include/ThreadSafeQueue.h

-#pragma once
-
-#include <queue>
-#include <mutex>
-#include <condition_variable>
-
-namespace ROSBridge {
-
-template <class T>
-class ThreadSafeQueue
-{
-public:
-    ThreadSafeQueue();
-    ~ThreadSafeQueue();
-
-    T pop_front();
-
-    void push_back(const T& item);
-    void push_back(T&& item);
-
-    int size();
-    bool empty();
-
-private:
-    std::deque<T> _queue;
-    std::mutex _mutex;
-    std::condition_variable _cond;
-};
-
-template <typename T>
-ThreadSafeQueue<T>::ThreadSafeQueue(){}
-
-template <typename T>
-ThreadSafeQueue<T>::~ThreadSafeQueue(){}
-
-template <typename T>
-T ThreadSafeQueue<T>::pop_front()
-{
-    std::unique_lock<std::mutex> mlock(_mutex);
-    while (_queue.empty())
-    {
-        _cond.wait(mlock);
-    }
-    T t = std::move(_queue.front());
-    _queue.pop_front();
-    return t;
-}
-
-template <typename T>
-void ThreadSafeQueue<T>::push_back(const T& item)
-{
-    std::unique_lock<std::mutex> mlock(_mutex);
-    _queue.push_back(item);
-    mlock.unlock();     // unlock before notificiation to minimize mutex con
-    _cond.notify_one(); // notify one waiting thread
-
-}
-
-template <typename T>
-void ThreadSafeQueue<T>::push_back(T&& item)
-{
-    std::unique_lock<std::mutex> mlock(_mutex);
-    _queue.push_back(std::move(item));
-    mlock.unlock();     // unlock before notificiation to minimize mutex con
-    _cond.notify_one(); // notify one waiting thread
-
-}
-
-template <typename T>
-int ThreadSafeQueue<T>::size()
-{
-    std::unique_lock<std::mutex> mlock(_mutex);
-    int size = _queue.size();
-    mlock.unlock();
-    return size;
-}
-
-} // namespace
-