123

qgroundcontrol.pro

@@ -28,11 +28,12 @@ QGCROOT  = $$PWD
 DebugBuild {
     DESTDIR  = $${OUT_PWD}/debug
     DEFINES += DEBUG
+    DEFINES += ROS_BRIDGE_DEBUG
     #DEFINES += ROS_BRIDGE_CLIENT_DEBUG
 }
 else {
     DESTDIR  = $${OUT_PWD}/release
-    DEFINES += DEBUG
+    DEFINES += ROS_BRIDGE_DEBUG
     DEFINES += NDEBUG
 }
 

src/Wima/Geometry/WimaPolygonArray.h

@@ -13,6 +13,9 @@ public:
     WimaPolygonArray(const WimaPolygonArray &other) :
         _polygons(other._polygons), _dirty(true)
     {}
+    ~WimaPolygonArray(){
+        _objs.clearAndDeleteContents();
+    }
 
     class QmlObjectListModel *QmlObjectListModel(){
         if (_dirty)
@@ -34,9 +37,9 @@ public:
 
 private:
     void _updateObjects(void){
-        _objs.clear();
+        _objs.clearAndDeleteContents();
         for (long i=0; i<_polygons.size(); ++i){
-            _objs.append(&_polygons[i]);
+            _objs.append(new PolygonType(_polygons[i]));
         }
     }
 

src/Wima/WimaController.cc

@@ -89,6 +89,7 @@ WimaController::WimaController(QObject *parent)
     , _batteryLevelTicker       (EVENT_TIMER_INTERVAL, 1000 /*ms*/)
     , _snakeTicker              (EVENT_TIMER_INTERVAL, 200 /*ms*/)
     , _nemoTimeoutTicker        (EVENT_TIMER_INTERVAL, 5000 /*ms*/)
+    , _topicServiceSetupDone    (false)
 {    
 
     // ROS Bridge.
@@ -552,22 +553,28 @@ bool WimaController::setWimaPlanData(const WimaPlanData &planData)
 
     if ( _enableSnake.rawValue().toBool()
          && _pRosBridge->isRunning()
-            && _pRosBridge->connected() ){
-        if ( _snakeTilesLocal.polygons().size() > 0 ){
+         && _pRosBridge->connected()
+         && _topicServiceSetupDone
+         && _snakeTilesLocal.polygons().size() > 0
+         )
+    {
         using namespace ros_bridge::messages;
         // Publish snake origin.
         JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-            Q_ASSERT(geographic_msgs::geo_point::toJson(
-                         _snakeOrigin, *jOrigin, jOrigin->GetAllocator()));
+        bool ret = geographic_msgs::geo_point::toJson(
+                     _snakeOrigin, *jOrigin, jOrigin->GetAllocator());
+        Q_ASSERT(ret);
         _pRosBridge->publish(std::move(jOrigin), "/snake/origin");
         // Publish snake tiles.
         JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-            Q_ASSERT(jsk_recognition_msgs::polygon_array::toJson(
-                         _snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator()));
+        ret = jsk_recognition_msgs::polygon_array::toJson(
+                     _snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator());
+        Q_ASSERT(ret);
         _pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
-        }
+
     }
 
+
     _localPlanDataValid = true;
     return true;
 }
@@ -758,21 +765,24 @@ void WimaController::_eventTimerHandler()
     }
 
     if ( _snakeTicker.ready() ) {
-        static bool setupDone = false;
         if ( _enableSnake.rawValue().toBool() ) {
             if ( !_pRosBridge->isRunning()) {
                 _pRosBridge->start();
-            } else if ( _pRosBridge->isRunning() && _pRosBridge->connected() && !setupDone) {
+            } else if ( _pRosBridge->isRunning()
+                        && _pRosBridge->connected()
+                        && !_topicServiceSetupDone) {
+                if ( _doTopicServiceSetup() )
+                    _topicServiceSetupDone = true;
+            } else if ( _pRosBridge->isRunning()
+                        && !_pRosBridge->connected()
+                        && _topicServiceSetupDone){
                 _pRosBridge->reset();
                 _pRosBridge->start();
-                _setupTopicService();
-                setupDone = true;
-            } else if ( _pRosBridge->isRunning() && !_pRosBridge->connected() ){
-                setupDone = false;
+                _topicServiceSetupDone = false;
             }
         } else  if ( _pRosBridge->isRunning() ) {
                 _pRosBridge->reset();
-                setupDone = false;
+                _topicServiceSetupDone = false;
         }
     }
 }
@@ -948,21 +958,35 @@ void WimaController::_switchSnakeManager(QVariant variant)
     }
 }
 
-void WimaController::_setupTopicService()
+bool WimaController::_doTopicServiceSetup()
 {
     using namespace ros_bridge::messages;
-    if ( _snakeTilesLocal.polygons().size() > 0){
+
+    if ( _snakeTilesLocal.polygons().size() == 0)
+        return false;
+
     // Publish snake origin.
+    _pRosBridge->advertiseTopic("/snake/origin",
+                                geographic_msgs::geo_point::messageType().c_str());
     JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-        Q_ASSERT(geographic_msgs::geo_point::toJson(
-                     _snakeOrigin, *jOrigin, jOrigin->GetAllocator()));
+    bool ret = geographic_msgs::geo_point::toJson(
+                _snakeOrigin, *jOrigin.get(), jOrigin->GetAllocator());
+    Q_ASSERT(ret);
+    (void)ret;
     _pRosBridge->publish(std::move(jOrigin), "/snake/origin");
+
+
     // Publish snake tiles.
+    _pRosBridge->advertiseTopic("/snake/tiles",
+                                jsk_recognition_msgs::polygon_array::messageType().c_str());
     JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-        Q_ASSERT(jsk_recognition_msgs::polygon_array::toJson(
-                     _snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator()));
+    ret = jsk_recognition_msgs::polygon_array::toJson(
+                _snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator());
+    Q_ASSERT(ret);
+    (void)ret;
     _pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
-    }
+
+
     // Subscribe nemo progress.
     _pRosBridge->subscribe("/nemo/progress",
                            /* callback */ [this](JsonDocUPtr pDoc){
@@ -972,23 +996,29 @@ void WimaController::_setupTopicService()
              || progress_msg.progress().size() != requiredSize ) { // Some error occured.
             // Set progress to default.
             progress_msg.progress().fill(0, requiredSize);            
-            // Publish origin and tiles again, if valid.
-            if ( this->_snakeTilesLocal.polygons().size() > 0){
+
             // Publish snake origin.
             JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-                Q_ASSERT(geographic_msgs::geo_point::toJson(
-                             this->_snakeOrigin, *jOrigin, jOrigin->GetAllocator()));
+            bool ret = geographic_msgs::geo_point::toJson(
+                         this->_snakeOrigin, *jOrigin, jOrigin->GetAllocator());
+            Q_ASSERT(ret);
+            (void)ret;
             this->_pRosBridge->publish(std::move(jOrigin), "/snake/origin");
+
             // Publish snake tiles.
             JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-                Q_ASSERT(jsk_recognition_msgs::polygon_array::toJson(
-                             this->_snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator()));
+            ret = jsk_recognition_msgs::polygon_array::toJson(
+                         this->_snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator());
+            Q_ASSERT(ret);
+            (void)ret;
             this->_pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
         }
-        }
 
         emit WimaController::nemoProgressChanged();
     });
+
+
+    // Subscribe /nemo/heartbeat.
     _pRosBridge->subscribe("/nemo/heartbeat",
                            /* callback */ [this](JsonDocUPtr pDoc){
         auto &heartbeat_msg = this->_nemoHeartbeat;
@@ -1004,31 +1034,38 @@ void WimaController::_setupTopicService()
         emit WimaController::nemoStatusStringChanged();
     });
 
+
+    // Advertise /snake/get_origin.
     _pRosBridge->advertiseService("/snake/get_origin", "snake_msgs/GetOrigin",
                                   [this](JsonDocUPtr) -> JsonDocUPtr
     {
         using namespace ros_bridge::messages;
+
         JsonDocUPtr pDoc(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
+        ::GeoPoint3D origin = this->_snakeOrigin;
         rapidjson::Value jOrigin(rapidjson::kObjectType);
-        if ( this->_snakeTilesLocal.polygons().size() > 0){
-            geographic_msgs::geo_point::toJson(
-                        this->_snakeOrigin, jOrigin, pDoc->GetAllocator());
-        } else {
-            geographic_msgs::geo_point::toJson(
-                        ::GeoPoint3D(0,0,0), jOrigin, pDoc->GetAllocator());
-        }
+        bool ret = geographic_msgs::geo_point::toJson(
+                    origin, jOrigin, pDoc->GetAllocator());
+        Q_ASSERT(ret);
+        (void)ret;
         pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator());
         return pDoc;
     });
 
+
+    // Advertise /snake/get_tiles.
     _pRosBridge->advertiseService("/snake/get_tiles", "snake_msgs/GetTiles",
                                   [this](JsonDocUPtr) -> JsonDocUPtr
     {
         JsonDocUPtr pDoc(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
         rapidjson::Value jSnakeTiles(rapidjson::kObjectType);
-        jsk_recognition_msgs::polygon_array::toJson(
+        bool ret = jsk_recognition_msgs::polygon_array::toJson(
                     this->_snakeTilesLocal, jSnakeTiles, pDoc->GetAllocator());
+        Q_ASSERT(ret);
+        (void)ret;
         pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator());
         return pDoc;
     });
+
+    return true;
 }

src/Wima/WimaController.h

@@ -337,7 +337,7 @@ private slots:
     void _snakeStoreWorkerResults            ();
     void _initStartSnakeWorker               ();
     void _switchSnakeManager                 (QVariant variant);
-    void _setupTopicService                     ();
+    bool _doTopicServiceSetup();
     // Periodic tasks.
     void _eventTimerHandler                  (void);
     // Waypoint manager handling.
@@ -407,6 +407,7 @@ private:
     int                     _fallbackStatus;
     ROSBridgePtr            _pRosBridge;
     static StatusMap        _nemoStatusMap;
+    bool                    _topicServiceSetupDone;
 
     // Periodic tasks.
     QTimer      _eventTimer;

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.cpp

@@ -18,18 +18,18 @@ struct Task{
 
 void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shared_ptr<WsClient> client, const std::__cxx11::string &message)
 {
-#ifndef DEBUG
+#ifndef ROS_BRIDGE_DEBUG
     (void)client_name;
 #endif
     if (!client->on_open)
     {
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         client->on_open = [client_name, message](std::shared_ptr<WsClient::Connection> connection) {
 #else
         client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
 #endif
 
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
             std::cout << client_name << ": Opened connection" << std::endl;
             std::cout << client_name << ": Sending message: " << message << std::endl;
 #endif
@@ -37,7 +37,7 @@ void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shar
         };
     }
 
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     if (!client->on_message)
     {
         client->on_message = [client_name](std::shared_ptr<WsClient::Connection> /*connection*/, std::shared_ptr<WsClient::InMessage> in_message) {
@@ -61,21 +61,21 @@ void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shar
     }
 
 #endif
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     std::thread client_thread([client_name, client]() {
 #else
     std::thread client_thread([client]() {
 #endif
         client->start();
 
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_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;
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         std::cout << client_name << " thread end" << std::endl;
 #endif
     });
@@ -122,7 +122,7 @@ void RosbridgeWsClient::run()
             // ====================================================================================
             if ( std::chrono::high_resolution_clock::now() > poll_time_point) {
                 poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval;
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
                 std::cout << "Starting new poll." << std::endl;
                 std::cout << "connected: " << this->isConnected->load() << std::endl;
 #endif
@@ -133,14 +133,14 @@ void RosbridgeWsClient::run()
                     return t.name == reset_status_task_name;
                 });
                 if ( it == task_list.end() ){
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
                     std::cout << "Adding status_task" << std::endl;
 #endif
                     // Check connection status.
                     auto status_set = std::make_shared<std::atomic_bool>(false);
                     auto status_client = std::make_shared<WsClient>(this->server_port_path);
                     status_client->on_open = [status_set, this](std::shared_ptr<WsClient::Connection>) {
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
                             std::cout << "status_client opened" << std::endl;
 #endif
                             this->isConnected->store(true);
@@ -190,16 +190,13 @@ void RosbridgeWsClient::run()
                     });
                     if ( topics_it == task_list.end() ){
                         // Call /rosapi/topics service.
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
                         std::cout << "Adding reset_topics_task" << std::endl;
 #endif
                         auto topics_set = std::make_shared<std::atomic_bool>(false);
                         this->callService("/rosapi/topics", [topics_set, this](
                                           std::shared_ptr<WsClient::Connection> connection,
                                           std::shared_ptr<WsClient::InMessage> in_message){
-#ifdef DEBUG
-                            std::cout << "/rosapi/topics: " << in_message->string() << std::endl;
-#endif
                             std::unique_lock<std::mutex> lk(this->mutex);
                             this->available_topics = in_message->string();
                             lk.unlock();
@@ -241,16 +238,13 @@ void RosbridgeWsClient::run()
                     });
                     if ( services_it == task_list.end() ){
                         // Call /rosapi/services service.
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
                         std::cout << "Adding reset_services_task" << std::endl;
 #endif
                         auto services_set = std::make_shared<std::atomic_bool>(false);
                         this->callService("/rosapi/services", [this, services_set](
                                           std::shared_ptr<WsClient::Connection> connection,
                                           std::shared_ptr<WsClient::InMessage> in_message){
-#ifdef DEBUG
-                            std::cout << "/rosapi/services: " << in_message->string() << std::endl;
-#endif
                             std::unique_lock<std::mutex> lk(this->mutex);
                             this->available_services = in_message->string();
                             lk.unlock();
@@ -295,26 +289,14 @@ void RosbridgeWsClient::run()
             // Process tasks.
             // ====================================================================================
             for ( auto task_it = task_list.begin(); task_it != task_list.end(); ){
-#ifdef DEBUG
-                std::cout << "processing task: " << task_it->name << std::endl;
-#endif
                 if ( !task_it->expired() ){
                     if ( task_it->ready() ){
-#ifdef DEBUG
-                        std::cout << "executing task: " << task_it->name << std::endl;
-#endif
                         task_it->execute();
                         task_it = task_list.erase(task_it);
                     } else {
-#ifdef DEBUG
-                        std::cout << "noting to do for task: " << task_it->name << std::endl;
-#endif
                         ++task_it;
                     }
                 } else {
-#ifdef DEBUG
-                    std::cout << "task expired: " << task_it->name << std::endl;
-#endif
                     task_it->clear_up();
                     task_it = task_list.erase(task_it);
                 }
@@ -328,7 +310,7 @@ void RosbridgeWsClient::run()
             task_it->clear_up();
         }
         task_list.clear();
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         std::cout << "periodic thread end" << std::endl;
 #endif
     });
@@ -349,9 +331,14 @@ void RosbridgeWsClient::reset()
     unsubscribeAll();
     unadvertiseAll();
     unadvertiseAllServices();
-    for (auto& client : client_map)
-    {
-        removeClient(client.first);
+
+    std::unique_lock<std::mutex> lk(mutex);
+    for (auto it = client_map.begin(); it != client_map.end(); ) {
+        std::string client_name = it->first;
+        lk.unlock();
+        removeClient(client_name);
+        lk.lock();
+        it = client_map.begin();
     }
 }
 
@@ -363,7 +350,7 @@ void RosbridgeWsClient::addClient(const std::string &client_name)
     {
         client_map[client_name] = std::make_shared<WsClient>(server_port_path);
     }
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     else
     {
         std::cerr << client_name << " has already been created" << std::endl;
@@ -391,7 +378,7 @@ void RosbridgeWsClient::stopClient(const std::string &client_name)
         // Stop the client asynchronously in 100 ms.
         // This is to ensure, that all threads involving the client have been launched.
         std::shared_ptr<WsClient> client = it->second;
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         std::thread t([client, client_name](){
 #else
         std::thread t([client](){
@@ -403,14 +390,13 @@ void RosbridgeWsClient::stopClient(const std::string &client_name)
 //            client->on_message = NULL;
 //            client->on_close = NULL;
 //            client->on_error = NULL;
-#ifdef DEBUG
-            std::cout << "removeClient thread: " << client_name << " reference count: " << client.use_count() << std::endl;
+#ifdef ROS_BRIDGE_DEBUG
             std::cout << client_name << " has been removed" << std::endl;
 #endif
         });
         t.detach();
     }
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     else
     {
         std::cerr << client_name << " has not been created" << std::endl;
@@ -428,7 +414,7 @@ void RosbridgeWsClient::removeClient(const std::string &client_name)
         {
             client_map.erase(it);
         }
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         else
         {
             std::cerr << client_name << " has not been created" << std::endl;
@@ -448,15 +434,24 @@ std::string RosbridgeWsClient::getAdvertisedServices(){
 }
 
 bool RosbridgeWsClient::topicAvailable(const std::string &topic){
-#ifdef DEBUG
+    std::lock_guard<std::mutex> lk(mutex);
+#ifdef ROS_BRIDGE_DEBUG
     std::cout << "checking if topic " << topic << " is available" << std::endl;
+    std::cout << "available topics: " << available_topics << std::endl;
 #endif
     size_t pos;
     {
-        std::lock_guard<std::mutex> lk(mutex);
         pos = available_topics.find(topic);
     }
-    return pos != std::string::npos ? true : false;
+    bool ret = pos != std::string::npos ? true : false;
+#ifdef ROS_BRIDGE_DEBUG
+    if ( ret ){
+        std::cout << "topic " << topic << " is available" << std::endl;
+    } else {
+        std::cout << "topic " << topic << " is not available" << std::endl;
+    }
+#endif
+    return ret;
 }
 
 void RosbridgeWsClient::advertise(const std::string &client_name, const std::string &topic, const std::string &type, const std::string &id)
@@ -471,7 +466,7 @@ void RosbridgeWsClient::advertise(const std::string &client_name, const std::str
             return topic == std::get<integral(EntryEnum::ServiceTopicName)>(td);
         });
         if ( it_ser_top != service_topic_list.end()){
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
             std::cerr << "topic: " << topic << " already advertised" << std::endl;
 #endif
             return;
@@ -487,13 +482,13 @@ void RosbridgeWsClient::advertise(const std::string &client_name, const std::str
         }
         message = "{" + message + "}";
 
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         client->on_open = [this, topic, message, client_name](std::shared_ptr<WsClient::Connection> connection) {
 #else
         client->on_open = [this, topic, message](std::shared_ptr<WsClient::Connection> connection) {
 #endif
 
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
             std::cout << client_name << ": Opened connection" << std::endl;
             std::cout << client_name << ": Sending message: " << message << std::endl;
 #endif
@@ -502,7 +497,7 @@ void RosbridgeWsClient::advertise(const std::string &client_name, const std::str
 
         start(client_name, client, message);
     }
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     else
     {
         std::cerr << client_name << "has not been created" << std::endl;
@@ -518,7 +513,7 @@ void RosbridgeWsClient::unadvertise(const std::string &topic, const std::string
         return topic == std::get<integral(EntryEnum::ServiceTopicName)>(td);
     });
     if ( it_ser_top == service_topic_list.end()){
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         std::cerr << "topic: " << topic << " not advertised" << std::endl;
 #endif
         return;
@@ -534,13 +529,13 @@ void RosbridgeWsClient::unadvertise(const std::string &topic, const std::string
 
     std::string client_name = "topic_unadvertiser" + topic;
     auto client = std::make_shared<WsClient>(server_port_path);
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     client->on_open = [client_name, message](std::shared_ptr<WsClient::Connection> connection) {
 #else
     client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
 #endif
 
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         std::cout << client_name << ": Opened connection" << std::endl;
         std::cout << client_name << ": Sending message: " << message << std::endl;
 #endif
@@ -569,7 +564,7 @@ void RosbridgeWsClient::publish(const std::string &topic, const rapidjson::Docum
         return topic == std::get<integral(EntryEnum::ServiceTopicName)>(td);
     });
     if ( it_ser_top == service_topic_list.end() ){
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         std::cerr << "topic: " << topic << " not yet advertised" << std::endl;
 #endif
         return;
@@ -588,12 +583,12 @@ void RosbridgeWsClient::publish(const std::string &topic, const rapidjson::Docum
     message = "{" + message + "}";
 
     std::shared_ptr<WsClient> publish_client = std::make_shared<WsClient>(server_port_path);
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     publish_client->on_open = [message, client_name](std::shared_ptr<WsClient::Connection> connection) {
 #else
     publish_client->on_open = [message, client_name](std::shared_ptr<WsClient::Connection> connection) {
 #endif
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         std::cout << client_name << ": Opened connection" << std::endl;
         std::cout << client_name << ": Sending message." << std::endl;
         std::cout << client_name << ": Sending message: " << message << std::endl;
@@ -619,7 +614,7 @@ void RosbridgeWsClient::subscribe(const std::string &client_name, const std::str
             return topic == std::get<integral(EntryEnum::ServiceTopicName)>(td);
         });
         if ( it_ser_top != service_topic_list.end()){
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
             std::cerr << "topic: " << topic << " already advertised" << std::endl;
 #endif
             return;
@@ -659,7 +654,7 @@ void RosbridgeWsClient::subscribe(const std::string &client_name, const std::str
         client->on_message = callback;
         this->start(client_name, client, message); // subscribe to topic
     }
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     else
     {
         std::cerr << client_name << "has not been created" << std::endl;
@@ -675,7 +670,7 @@ void RosbridgeWsClient::unsubscribe(const std::string &topic, const std::string
         return topic == std::get<integral(EntryEnum::ServiceTopicName)>(td);
     });
     if ( it_ser_top == service_topic_list.end()){
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         std::cerr << "topic: " << topic << " not advertised" << std::endl;
 #endif
         return;
@@ -691,13 +686,13 @@ void RosbridgeWsClient::unsubscribe(const std::string &topic, const std::string
 
     std::string client_name = "topic_unsubscriber" + topic;
     auto client = std::make_shared<WsClient>(server_port_path);
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     client->on_open = [client_name, message](std::shared_ptr<WsClient::Connection> connection) {
 #else
     client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
 #endif
 
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         std::cout << client_name << ": Opened connection" << std::endl;
         std::cout << client_name << ": Sending message: " << message << std::endl;
 #endif
@@ -729,7 +724,7 @@ void RosbridgeWsClient::advertiseService(const std::string &client_name, const s
             return service == std::get<integral(EntryEnum::ServiceTopicName)>(td);
         });
         if ( it_ser_top != service_topic_list.end()){
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
             std::cerr << "service: " << service << " already advertised" << std::endl;
 #endif
             return;
@@ -743,7 +738,7 @@ void RosbridgeWsClient::advertiseService(const std::string &client_name, const s
         it_client->second->on_message = callback;
         start(client_name, it_client->second, message);
     }
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     else
     {
         std::cerr << client_name << "has not been created" << std::endl;
@@ -759,7 +754,7 @@ void RosbridgeWsClient::unadvertiseService(const std::string &service){
         return service == std::get<integral(EntryEnum::ServiceTopicName)>(td);
     });
     if ( it_ser_top == service_topic_list.end()){
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         std::cerr << "service: " << service << " not advertised" << std::endl;
 #endif
         return;
@@ -771,13 +766,13 @@ void RosbridgeWsClient::unadvertiseService(const std::string &service){
 
     std::string client_name = "service_unadvertiser" + service;
     auto client = std::make_shared<WsClient>(server_port_path);
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     client->on_open = [client_name, message](std::shared_ptr<WsClient::Connection> connection) {
 #else
     client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
 #endif
 
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         std::cout << client_name << ": Opened connection" << std::endl;
         std::cout << client_name << ": Sending message: " << message << std::endl;
 #endif
@@ -816,12 +811,12 @@ void RosbridgeWsClient::serviceResponse(const std::string &service, const std::s
     std::string client_name = "service_response_client" + service;
     std::shared_ptr<WsClient> service_response_client = std::make_shared<WsClient>(server_port_path);
 
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     service_response_client->on_open = [message, client_name](std::shared_ptr<WsClient::Connection> connection) {
 #else
     service_response_client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
 #endif
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         std::cout << client_name << ": Opened connection" << std::endl;
         std::cout << client_name << ": Sending message: " << message << std::endl;
 #endif
@@ -869,7 +864,7 @@ void RosbridgeWsClient::callService(const std::string &service, const InMessage
     else
     {
         call_service_client->on_message = [client_name](std::shared_ptr<WsClient::Connection> connection, std::shared_ptr<WsClient::InMessage> in_message) {
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
             std::cout << client_name << ": Message received: " << in_message->string() << std::endl;
             std::cout << client_name << ": Sending close connection" << std::endl;
 #else
@@ -884,7 +879,7 @@ void RosbridgeWsClient::callService(const std::string &service, const InMessage
 
 bool RosbridgeWsClient::serviceAvailable(const std::string &service)
 {
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     std::cout << "checking if service " << service << " is available" << std::endl;
 #endif
     size_t pos;
@@ -892,7 +887,16 @@ bool RosbridgeWsClient::serviceAvailable(const std::string &service)
         std::lock_guard<std::mutex> lk(mutex);
         pos = available_services.find(service);
     }
-    return pos != std::string::npos ? true : false;
+    bool ret = pos != std::string::npos ? true : false;
+#ifdef ROS_BRIDGE_DEBUG
+    if ( ret ){
+        std::cout << "service " << service << " is available" << std::endl;
+    } else {
+        std::cout << "service " << service << " is not available" << std::endl;
+
+    }
+#endif
+    return ret;
 }
 
 void RosbridgeWsClient::waitForService(const std::string &service)
@@ -904,28 +908,27 @@ void RosbridgeWsClient::waitForService(const std::string &service)
 
 void RosbridgeWsClient::waitForService(const std::string &service, const std::function<bool(void)> stop)
 {
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     auto s = std::chrono::high_resolution_clock::now();
     long counter = 0;
 #endif
     const auto poll_interval = std::chrono::milliseconds(1000);
-    auto poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval;
+    auto poll_time_point = std::chrono::high_resolution_clock::now();
     while( !stop() )
     {
         if ( std::chrono::high_resolution_clock::now() > poll_time_point ){
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
             ++counter;
 #endif
             if ( serviceAvailable(service) ){
                 break;
-            } else {
-                poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval;
             }
+            poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval;
         } else {
             std::this_thread::sleep_for(std::chrono::milliseconds(1));
         }
     };
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     auto e = std::chrono::high_resolution_clock::now();
     std::cout << "waitForService() " << service << " time: "
               << std::chrono::duration_cast<std::chrono::milliseconds>(e-s).count()
@@ -944,28 +947,27 @@ void RosbridgeWsClient::waitForTopic(const std::string &topic)
 
 void RosbridgeWsClient::waitForTopic(const std::string &topic, const std::function<bool(void)> stop)
 {
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     auto s = std::chrono::high_resolution_clock::now();
     long counter = 0;
 #endif
     const auto poll_interval = std::chrono::milliseconds(1000);
-    auto poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval;
+    auto poll_time_point = std::chrono::high_resolution_clock::now();
     while( !stop() )
     {
         if ( std::chrono::high_resolution_clock::now() > poll_time_point ){
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
             ++counter;
 #endif
             if ( topicAvailable(topic) ){
                 break;
-            } else {
-                poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval;
             }
+            poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval;
         } else {
             std::this_thread::sleep_for(std::chrono::milliseconds(1));
         }
     };
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     auto e = std::chrono::high_resolution_clock::now();
     std::cout << "waitForTopic() " << topic << " time: "
               << std::chrono::duration_cast<std::chrono::milliseconds>(e-s).count()

src/comm/ros_bridge/include/com_private.cpp

@@ -20,18 +20,10 @@ bool ros_bridge::com_private::getTopic(const ros_bridge::com_private::JsonDoc &d
 {
     if ( doc.HasMember("topic") && doc["topic"].IsString() ){
 
-
-        rapidjson::StringBuffer sb1;
-        rapidjson::Writer<rapidjson::StringBuffer> writer1(sb1);
-        doc.Accept(writer1);
-        std::cout << "getTopic doc: " << sb1.GetString() << std::endl;
-
         rapidjson::StringBuffer sb;
         rapidjson::Writer<rapidjson::StringBuffer> writer(sb);
         doc["topic"].Accept(writer);
         topic = sb.GetString();
-
-        std::cout << "getTopic topic: " << sb.GetString() << std::endl;
         return true;
     } else
         return false;

src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.cpp

 #include "geopoint.h"
 
 std::string ros_bridge::messages::geographic_msgs::geo_point::messageType(){
-    return "geometry_msgs/GeoPoint";
+    return "geographic_msgs/GeoPoint";
 }

src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h

@@ -109,9 +109,6 @@ bool toJson(const T&p, rapidjson::Value &value, rapidjson::Document::AllocatorTy
     auto altitude = detail::getAltitude(p, traits::Type2Type<Components>()); // If T has no member altitude() replace it by 0.0;
 
     value.AddMember("altitude", rapidjson::Value().SetFloat((_Float64)altitude), allocator);
-    rapidjson::Value jType;
-    jType.SetString(messageType().c_str(), allocator);
-    value.AddMember("type", jType, allocator);
     return true;
 }
 

src/comm/ros_bridge/include/messages/geometry_msgs/point32.h

@@ -103,9 +103,6 @@ bool toJson(const T&p, rapidjson::Value &value, rapidjson::Document::AllocatorTy
     auto z = detail::getZ(p, Type2Type<Components>()); // If T has no member z() replace it by 0.
 
     value.AddMember("z", rapidjson::Value().SetFloat(z), allocator);
-    rapidjson::Value jType;
-    jType.SetString(messageType().c_str(), allocator);
-    value.AddMember("type", jType, allocator);
     return true;
 }
 

src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h

@@ -57,9 +57,6 @@ bool toJson(const PolygonType &poly, rapidjson::Value &value, rapidjson::Documen
     }
 
     value.AddMember("points", points, allocator);
-    rapidjson::Value jType;
-    jType.SetString(messageType().c_str(), allocator);
-    value.AddMember("type", jType, allocator);
     return true;
 }
 

src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h

@@ -95,9 +95,6 @@ bool _toJson(const PolygonType &poly,
     }
     value.AddMember("header", header, allocator);
     value.AddMember("polygon", polygon, allocator);
-    rapidjson::Value jType;
-    jType.SetString(messageType().c_str(), allocator);
-    value.AddMember("type", jType, allocator);
 
     return true;
 }

src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h

@@ -183,10 +183,6 @@ namespace detail {
         detail::likelihoodToJson(p, jLikelihood, allocator, traits::Int2Type<HasLikelihood::value>());
         value.AddMember("likelihood", jLikelihood, allocator);
 
-        rapidjson::Value jType;
-        jType.SetString(messageType().c_str(), allocator);
-        value.AddMember("type", jType, allocator);
-
         return true;
     }
 

src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h

@@ -32,9 +32,6 @@ template <class HeartbeatType>
 bool toJson(const HeartbeatType &heartbeat, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
 {
     value.AddMember("status", std::int32_t(heartbeat.status()), allocator);
-    rapidjson::Value jType;
-    jType.SetString(messageType().c_str(), allocator);
-    value.AddMember("type", jType, allocator);
     return true;
 }
 

src/comm/ros_bridge/include/messages/nemo_msgs/progress.h

@@ -41,9 +41,6 @@ bool toJson(const ProgressType &p, rapidjson::Value &value, rapidjson::Document:
         jProgress.PushBack(rapidjson::Value().SetInt(std::int32_t(p.progress()[i])), allocator);
     }
     value.AddMember("progress", jProgress, allocator);
-    rapidjson::Value jType;
-    jType.SetString(messageType().c_str(), allocator);
-    value.AddMember("type", jType, allocator);
     return true;
 }
 

src/comm/ros_bridge/include/messages/std_msgs/header.h

@@ -62,9 +62,6 @@ bool toJson(const HeaderType &header,
                                                header.frameId().length(),
                                                allocator),
                   allocator);
-    rapidjson::Value jType;
-    jType.SetString(messageType().c_str(), allocator);
-    value.AddMember("type", jType, allocator);
 
     return true;
 }

src/comm/ros_bridge/include/messages/std_msgs/time.h

@@ -37,9 +37,6 @@ bool toJson(const TimeType &time,
 {
     value.AddMember("secs", rapidjson::Value().SetUint(uint32_t(time.secs())), allocator);
     value.AddMember("nsecs", rapidjson::Value().SetUint(uint32_t(time.nSecs())), allocator);
-    rapidjson::Value jType;
-    jType.SetString(messageType().c_str(), allocator);
-    value.AddMember("type", jType, allocator);
 
     return true;
 }

src/comm/ros_bridge/include/ros_bridge.h

@@ -23,13 +23,16 @@ public:
     void advertiseService(const char* service,
                           const char* type,
                           const std::function<JsonDocUPtr(JsonDocUPtr)> &callback);
+    void advertiseTopic(const char* topic,
+                        const char* type);
 
     //! @brief Start ROS bridge.
     void start();
     //! @brief Stops ROS bridge.
     void reset();
+
     //! @return Returns true if connected to the rosbridge_server, false either.
-    //! @note This function can block up to 100ms. However normal execution time is smaller.
+    //! @note \fn calls start().
     bool connected();
     bool isRunning();
 

src/comm/ros_bridge/include/topic_publisher.cpp

@@ -20,8 +20,6 @@ void ros_bridge::com_private::TopicPublisher::start()
         return;
     _stopped->store(false);
     _pThread = std::make_unique<std::thread>([this]{
-        // Init.
-        std::unordered_map<std::string, std::string> topicMap;
         // Main Loop.
         while( !this->_stopped->load() ){
             std::unique_lock<std::mutex> lk(this->_mutex);
@@ -39,38 +37,25 @@ void ros_bridge::com_private::TopicPublisher::start()
 
             // Get topic and type from Json message and remove it.
             std::string topic;
-            assert(com_private::getTopic(*pJsonDoc, topic));
-            assert(com_private::removeTopic(*pJsonDoc));
-            std::string type;
-            assert(com_private::getType(*pJsonDoc, type));
-            assert(com_private::removeType(*pJsonDoc));
+            bool ret = com_private::getTopic(*pJsonDoc, topic);
+            assert(ret);
+            (void)ret;
+            ret = com_private::removeTopic(*pJsonDoc);
+            assert(ret);
+            (void)ret;
 
-            // Check if topic must be advertised.
-            std::string clientName =
-                    ros_bridge::com_private::_topicAdvertiserKey
-                    + topic;
-            auto it =  topicMap.find(clientName);
-            if ( it ==  topicMap.end()) { // Need to advertise topic?
-                 topicMap.insert(std::make_pair(clientName, topic));
-                 this->_rbc.addClient(clientName);
-                 this->_rbc.advertise(clientName, topic, type);
+            // Wait for topic (Rosbridge needs some time to process a advertise() request).
             this->_rbc.waitForTopic(topic, [this]{
                 return this->_stopped->load();
-                 }); // Wait until topic is advertised.
-            }
+            });
 
             // Publish Json message.
             if ( !this->_stopped->load() )
                 this->_rbc.publish(topic, *pJsonDoc);
         } // while loop
-
-        // Tidy up.
-        for (auto& it :  topicMap){
-            this->_rbc.unadvertise(it.second);
-            this->_rbc.removeClient(it.first);
-        }
-
+#ifdef ROS_BRIDGE_DEBUG
         std::cout << "TopicPublisher: publisher thread terminated." << std::endl;
+#endif
     });
 }
 
@@ -88,6 +73,12 @@ void ros_bridge::com_private::TopicPublisher::reset()
     _pThread->join();
 
     lk.lock();    
+    // Tidy up.
+    for (auto& it :  this->_topicMap){
+        this->_rbc.unadvertise(it.first);
+        this->_rbc.removeClient(it.second);
+    }
+    this->_topicMap.clear();
     _queue.clear();
 }
 
@@ -95,14 +86,52 @@ void ros_bridge::com_private::TopicPublisher::publish(
         ros_bridge::com_private::JsonDocUPtr docPtr,
         const char *topic)
 {
-    std::cout << "TopicPublisher \"topic\": " << topic << std::endl;
+    // Check if topic was advertised.
+    std::string t(topic);
+    std::unique_lock<std::mutex> lk(this->_mutex);
+    auto it =  this->_topicMap.find(t);
+    if ( it ==  this->_topicMap.end()) { // Not yet advertised?
+        lk.unlock();
+#ifdef ROS_BRIDGE_DEBUG
+        std::cerr << "TopicPublisher: topic "
+                  << t << " not yet advertised" << std::endl;
+#endif
+        return;
+    }
+    lk.unlock();
+
+    // Add topic information to json doc.
     auto &allocator = docPtr->GetAllocator();
     rapidjson::Value key("topic", allocator);
     rapidjson::Value value(topic, allocator);
     docPtr->AddMember(key, value, allocator);
-    std::unique_lock<std::mutex> lock(_mutex);
+
+    lk.lock();
     _queue.push_back(std::move(docPtr));
-    lock.unlock();
+    lk.unlock();
     _cv.notify_one();  // Wake publisher thread.
 }
 
+bool ros_bridge::com_private::TopicPublisher::advertise(const char *topic, const char *type)
+{
+    std::unique_lock<std::mutex> lk(this->_mutex);
+    std::string t(topic);
+    auto it =  this->_topicMap.find(t);
+    if ( it ==  this->_topicMap.end()) { // Need to advertise topic?
+        std::string clientName =
+                std::string(ros_bridge::com_private::_topicAdvertiserKey)
+                + t;
+        this->_topicMap.insert(std::make_pair(t, clientName));
+        lk.unlock();
+        this->_rbc.addClient(clientName);
+        this->_rbc.advertise(clientName, t, type);
+        return true;
+    } else {
+        lk.unlock();
+#if ROS_BRIDGE_DEBUG
+        std::cerr << "TopicPublisher: topic " << topic << " already advertised" << std::endl;
+#endif
+        return false;
+    }
+}
+

src/comm/ros_bridge/include/topic_publisher.h

@@ -31,9 +31,12 @@ public:
     void reset();
 
     void publish(JsonDocUPtr docPtr, const char *topic);
+    bool advertise(const char *topic, const char *type);
 
 private:
+    using TopicMap = std::unordered_map<std::string, std::string>;
     JsonQueue           _queue;
+    TopicMap            _topicMap;
     std::mutex          _mutex;
     std::shared_ptr<std::atomic_bool>   _stopped;
     RosbridgeWsClient  &_rbc;

src/comm/ros_bridge/src/ros_bridge.cpp

@@ -32,8 +32,15 @@ void ros_bridge::ROSBridge::advertiseService(const char *service,
     _server.advertiseService(service, type, callback);
 }
 
+void ros_bridge::ROSBridge::advertiseTopic(const char *topic, const char *type)
+{
+    _topicPublisher.advertise(topic, type);
+}
+
 void ros_bridge::ROSBridge::start()
 {
+    if ( !_stopped->load() )
+        return;
     _stopped->store(false);
     _rbc.run();
     _topicPublisher.start();
@@ -44,6 +51,8 @@ void ros_bridge::ROSBridge::start()
 void ros_bridge::ROSBridge::reset()
 {
     auto start = std::chrono::high_resolution_clock::now();
+    if ( _stopped->load() )
+        return;
     _stopped->store(true);
     _topicPublisher.reset();
     _topicSubscriber.reset();
@@ -56,6 +65,7 @@ void ros_bridge::ROSBridge::reset()
 
 bool ros_bridge::ROSBridge::connected()
 {
+    start();
     return _rbc.connected();
 }