diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index 5ede3adcd72f05ee4acd6b9759d12a15dfa2b3ee..2b11cb8b0e56a43351d472b682eef694aad79c08 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -27,6 +27,8 @@ QGCROOT  = $$PWD
 
 DebugBuild {
     DESTDIR  = $${OUT_PWD}/debug
+    #DEFINES += DEBUG
+    #DEFINES += ROS_BRIDGE_CLIENT_DEBUG
 }
 else {
     DESTDIR  = $${OUT_PWD}/release
diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml
index e51f73b4486c934049c99529efdb8eae78a3c101..3a08cab9fbd6d87c0a12795b91a5235844324951 100644
--- a/src/FlightDisplay/FlightDisplayViewMap.qml
+++ b/src/FlightDisplay/FlightDisplayViewMap.qml
@@ -251,7 +251,8 @@ FlightMap {
 
         delegate: MapCircle{
             center:         modelData
-            border.color:   "transparent"
+            border.color:   "black"
+            border.width:   1
             color:          getColor(wimaController.nemoProgress[index])
             radius:         0.6
             opacity:        1
diff --git a/src/Snake/snake.cpp b/src/Snake/snake.cpp
index 015166814fdc7d71db77bc305e0b6d8b36a07dcf..1b079432fe68c2c7cb4a08cc20b64103c5c9ebc9 100644
--- a/src/Snake/snake.cpp
+++ b/src/Snake/snake.cpp
@@ -4,6 +4,13 @@
 #include "clipper/clipper.hpp"
 #define CLIPPER_SCALE 10000
 
+#include "ortools/constraint_solver/routing.h"
+#include "ortools/constraint_solver/routing_enums.pb.h"
+#include "ortools/constraint_solver/routing_index_manager.h"
+#include "ortools/constraint_solver/routing_parameters.h"
+
+using namespace operations_research;
+
 #ifndef NDEBUG
 //#define SHOW_TIME
 #endif
@@ -37,7 +44,7 @@ bool Scenario::addArea(Area &area)
         return false;
 }
 
-bool Scenario::defined(double tileWidth, double tileHeight, double minTileArea)
+bool Scenario::update(double tileWidth, double tileHeight, double minTileArea)
 {
     if (!_areas2enu())
         return false;
@@ -301,6 +308,14 @@ bool Scenario::_calculateJoinedArea()
     return true;
 }
 
+
+
+struct FlightPlan::RoutingDataModel{
+    Matrix<int64_t>                 distanceMatrix;
+    long                            numVehicles;
+    RoutingIndexManager::NodeIndex  depot;
+};
+
 FlightPlan::FlightPlan()
 {
 
@@ -356,7 +371,7 @@ bool FlightPlan::generate(double lineDistance, double minTransectLength)
     }
     size_t n1 = vertices.size();
     // Generate routing model.
-    RoutingDataModel_t dataModel;
+    RoutingDataModel dataModel;
     Matrix<double> connectionGraph(n1, n1);
 
 #ifdef SHOW_TIME
@@ -612,7 +627,7 @@ bool FlightPlan::_generateTransects(double lineDistance, double minTransectLengt
 void FlightPlan::_generateRoutingModel(const BoostLineString &vertices,
                                        const BoostPolygon &polygonOffset,
                                        size_t n0,
-                                       FlightPlan::RoutingDataModel_t &dataModel,
+                                       FlightPlan::RoutingDataModel &dataModel,
                                        Matrix<double> &graph)
 {
 #ifdef SHOW_TIME
diff --git a/src/Snake/snake.h b/src/Snake/snake.h
index c5ea01e1fb80ca694addeb04cc9e395e23f438cc..923b1662b5ee89cb8c23e5d71155a035b0b9a13d 100644
--- a/src/Snake/snake.h
+++ b/src/Snake/snake.h
@@ -6,16 +6,9 @@
 
 #include "snake_geometry.h"
 
-#include "ortools/constraint_solver/routing.h"
-#include "ortools/constraint_solver/routing_enums.pb.h"
-#include "ortools/constraint_solver/routing_index_manager.h"
-#include "ortools/constraint_solver/routing_parameters.h"
-
 using namespace std;
 using namespace snake_geometry;
 
-using namespace operations_research;
-
 namespace snake {
 
     enum AreaType {MeasurementArea, ServiceArea, Corridor};
@@ -60,7 +53,7 @@ namespace snake {
         const vector<GeoPoint3D>        &getTileCenterPoints()    {return _tileCenterPoints;}
         const GeoPoint3D                &getHomePositon()         {return _homePosition;}
 
-        bool defined(double tileWidth, double tileHeight, double minTileArea);
+        bool update(double tileWidth, double tileHeight, double minTileArea);
 
         string errorString;
 
@@ -104,8 +97,8 @@ namespace snake {
     public:
         FlightPlan();
 
-        void setScenario(const Scenario &scenario)          {_scenario = scenario;}
-        void setProgress(const vector<int8_t> &progress)    {_progress = progress;}
+        void setScenario(Scenario &scenario)          {_scenario = scenario;}
+        void setProgress(vector<int> &progress)       {_progress = progress;}
 
         const Scenario          &getScenario(void)          const {return _scenario;}
         const BoostLineString   &getWaypointsENU(void)      const {return _waypointsENU;}
@@ -124,28 +117,22 @@ namespace snake {
     private:
         // Search Filter to speed up routing.SolveWithParameters(...);
         // Found here: http://www.lia.disi.unibo.it/Staff/MicheleLombardi/or-tools-doc/user_manual/manual/ls/ls_filtering.html
-        class SearchFilter : public LocalSearchFilter{
-
-        };
+        class SearchFilter;
 
-        typedef struct{
-            Matrix<int64_t>                 distanceMatrix;
-            long                            numVehicles;
-            RoutingIndexManager::NodeIndex  depot;
-        }RoutingDataModel_t;
+        struct RoutingDataModel;
 
         bool _generateTransects(double lineDistance, double minTransectLength);
         void _generateRoutingModel(const BoostLineString &vertices,
                                    const BoostPolygon &polygonOffset,
                                    size_t n0,
-                                   RoutingDataModel_t &dataModel,
+                                   RoutingDataModel &dataModel,
                                    Matrix<double> &graph);
 
         Scenario                                _scenario;
         BoostLineString                         _waypointsENU;
         GeoPoint2DList                          _waypoints;
         vector<BoostLineString>                 _transects;
-        vector<int8_t>                          _progress;
+        vector<int>                             _progress;
         vector<uint64_t>                        _arrivalPathIdx;
         vector<uint64_t>                        _returnPathIdx;
 
diff --git a/src/Wima/Geometry/GeoPoint3D.cpp b/src/Wima/Geometry/GeoPoint3D.cpp
index 5f0165866e88da5c2c3d188ef1dc710305ef1d9f..2d75e61d312da1d10cc93db96f3ab79ea335f39c 100644
--- a/src/Wima/Geometry/GeoPoint3D.cpp
+++ b/src/Wima/Geometry/GeoPoint3D.cpp
@@ -1,30 +1,30 @@
 #include "GeoPoint3D.h"
 
-GeoPoint3D::GeoPoint3D(QObject *parent)
+GeoPoint::GeoPoint(QObject *parent)
     : QObject(parent), ROSGeoPoint() {}
 
-GeoPoint3D::GeoPoint3D(double latitude, double longitude, double altitude, QObject *parent)
+GeoPoint::GeoPoint(double latitude, double longitude, double altitude, QObject *parent)
     : QObject(parent), ROSGeoPoint(latitude, longitude, altitude)
 {}
 
-GeoPoint3D::GeoPoint3D(const GeoPoint3D &p, QObject *parent)
+GeoPoint::GeoPoint(const GeoPoint &p, QObject *parent)
     : QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
 {}
 
-GeoPoint3D::GeoPoint3D(const ROSGeoPoint &p, QObject *parent)
+GeoPoint::GeoPoint(const ROSGeoPoint &p, QObject *parent)
     : QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
 {}
 
-GeoPoint3D::GeoPoint3D(const QGeoCoordinate &p, QObject *parent)
+GeoPoint::GeoPoint(const QGeoCoordinate &p, QObject *parent)
     : QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
 {}
 
-GeoPoint3D *GeoPoint3D::Clone() const
+GeoPoint *GeoPoint::Clone() const
 {
-    return new GeoPoint3D(*this);
+    return new GeoPoint(*this);
 }
 
-GeoPoint3D &GeoPoint3D::operator=(const GeoPoint3D &p)
+GeoPoint &GeoPoint::operator=(const GeoPoint &p)
 {
     this->setLatitude(p.latitude());
     this->setLongitude(p.longitude());
@@ -32,7 +32,7 @@ GeoPoint3D &GeoPoint3D::operator=(const GeoPoint3D &p)
 
     return *this;
 }
-GeoPoint3D &GeoPoint3D::operator=(const QGeoCoordinate &p)
+GeoPoint &GeoPoint::operator=(const QGeoCoordinate &p)
 {
     this->setLatitude(p.latitude());
     this->setLongitude(p.longitude());
diff --git a/src/Wima/Geometry/GeoPoint3D.h b/src/Wima/Geometry/GeoPoint3D.h
index 407297003ca40379e281d31a557f3b09d08b5252..5579b63b2ce9a7933c74017c76f0806820ae5521 100644
--- a/src/Wima/Geometry/GeoPoint3D.h
+++ b/src/Wima/Geometry/GeoPoint3D.h
@@ -11,27 +11,27 @@
 typedef ROSBridge::MessageBaseClass ROSMsg;
 typedef ROSBridge::GenericMessages::GeographicMsgs::GeoPoint ROSGeoPoint;
 namespace MsgGroups = ROSBridge::MessageGroups;
-class GeoPoint3D : public QObject, public ROSGeoPoint
+class GeoPoint : public QObject, public ROSGeoPoint
 {
     Q_OBJECT
 public:
     typedef MsgGroups::GeoPointGroup Group;
 
-    GeoPoint3D(QObject *parent = nullptr);
-    GeoPoint3D(double latitude,
+    GeoPoint(QObject *parent = nullptr);
+    GeoPoint(double latitude,
                double longitude,
                double altitude,
                QObject *parent = nullptr);
-    GeoPoint3D(const GeoPoint3D& p,
+    GeoPoint(const GeoPoint& p,
                QObject *parent = nullptr);
-    GeoPoint3D(const ROSGeoPoint& p,
+    GeoPoint(const ROSGeoPoint& p,
                QObject *parent = nullptr);
-    GeoPoint3D(const QGeoCoordinate& p,
+    GeoPoint(const QGeoCoordinate& p,
                QObject *parent = nullptr);
 
-    virtual GeoPoint3D *Clone() const override;
-    GeoPoint3D &operator=(const GeoPoint3D&p);
-    GeoPoint3D &operator=(const QGeoCoordinate&p);
+    virtual GeoPoint *Clone() const override;
+    GeoPoint &operator=(const GeoPoint&p);
+    GeoPoint &operator=(const QGeoCoordinate&p);
 
 };
 
diff --git a/src/Wima/Snake/SnakeWorker.cc b/src/Wima/Snake/SnakeWorker.cc
index e82bf3d64723118897d82ef45a605163e30fb806..1cbe45cba0e59ef6f92094ae91f8482ad52359ae 100644
--- a/src/Wima/Snake/SnakeWorker.cc
+++ b/src/Wima/Snake/SnakeWorker.cc
@@ -2,58 +2,84 @@
 #include <QGeoCoordinate>
 
 
+#include "snake_geometry.h"
+#include "snake.h"
+
+
+
+using namespace snake;
+using namespace snake_geometry;
+
 SnakeWorker::SnakeWorker(QObject *parent) : QThread(parent)
-  , _lineDistance       (3) // meter
-  , _minTransectLength  (2) // meter
+    , _lineDistance(3)
+    , _minTransectLength(2)
+    , _tileWidth(5)
+    , _tileHeight(5)
+    , _minTileArea(3)
 {
 }
 
 SnakeWorker::~SnakeWorker()
 {
-}
 
-void SnakeWorker::setScenario(const Scenario &scenario)
-{
-    _scenario = scenario;
 }
 
-void SnakeWorker::setProgress(const QVector<int> &progress)
+WorkerResult_t &SnakeWorker::result()
 {
-    _progress.clear();
-    for (auto p : progress) {
-        assert(p >= -1 && p <= 100);
-        _progress.push_back(p);
-    }
+    return _result;
 }
 
-void SnakeWorker::setLineDistance(double lineDistance)
+bool SnakeWorker::precondition() const
 {
-    assert(lineDistance > 0);
-    _lineDistance = lineDistance;
-}
 
-void SnakeWorker::setMinTransectLength(double minTransectLength)
-{
-    assert(minTransectLength > 0);
-    _minTransectLength = minTransectLength;
+    return _mArea.size() > 0
+            && _sArea.size() > 0
+            && _corridor.size() > 0
+            && _progress.size() > 0;
 }
 
-const WorkerResult_t &SnakeWorker::getResult() const
+double SnakeWorker::lineDistance() const
 {
-    return _result;
+    return _lineDistance;
 }
 
 void SnakeWorker::run()
 {
     // Reset _result struct.
-    _result.success = false;
-    _result.errorMessage = "";
-    _result.waypoints.clear();
-    _result.returnPathIdx.clear();
-    _result.arrivalPathIdx.clear();
+    _result.clear();
+
+    if ( !precondition())
+        return;
+
+    Scenario scenario;// Initialize scenario.
+
+    Area mArea;
+    for (auto vertex : _mArea){
+        mArea.geoPolygon.push_back(GeoPoint2D{vertex.latitude(), vertex.longitude()});
+    }
+    mArea.type = AreaType::MeasurementArea;
+
+    Area sArea;
+    for (auto vertex : _sArea){
+        sArea.geoPolygon.push_back(GeoPoint2D{vertex.latitude(), vertex.longitude()});
+    }
+    sArea.type = AreaType::ServiceArea;
+
+    Area corridor;
+    for (auto vertex : _corridor){
+        corridor.geoPolygon.push_back(GeoPoint2D{vertex.latitude(), vertex.longitude()});
+    }
+    corridor.type = AreaType::Corridor;
+
+    scenario.addArea(mArea);
+    scenario.addArea(sArea);
+    scenario.addArea(corridor);
+
+    if ( !scenario.update(_tileWidth, _tileHeight, _minTileArea) )
+        return;
 
     FlightPlan flightplan;
-    flightplan.setScenario(_scenario);
+    flightplan.setScenario(scenario);
     flightplan.setProgress(_progress);
 
     // Trying to generate flight plan.
@@ -67,20 +93,116 @@ void SnakeWorker::run()
         _result.success = true;
 
         // Fill result struct.
-        auto waypoints = flightplan.getWaypoints();
+        auto& waypoints = flightplan.getWaypoints();
         for (auto vertex : waypoints){
-            _result.waypoints.append(QVariant::fromValue(QGeoCoordinate(vertex[0], vertex[1], 0)));
+            _result.waypoints.append(QGeoCoordinate(vertex[0], vertex[1], 0));
         }
 
-        auto arrivalPathIdx = flightplan.getArrivalPathIdx();
+        auto& arrivalPathIdx = flightplan.getArrivalPathIdx();
         for (auto idx : arrivalPathIdx){
             _result.arrivalPathIdx.append(idx);
         }
 
-        auto returnPathIdx = flightplan.getReturnPathIdx();
+        auto& returnPathIdx = flightplan.getReturnPathIdx();
         for (auto idx : returnPathIdx){
             _result.returnPathIdx.append(idx);
         }
 
+
+        // Get tiles and origin.
+        auto origin = scenario.getOrigin();
+        _result.origin.setLatitude(origin[0]);
+        _result.origin.setLongitude(origin[1]);
+        _result.origin.setAltitude(origin[2]);
+        const auto &tiles = scenario.getTiles();
+        const auto &cps = scenario.getTileCenterPoints();
+        for ( unsigned int i=0; i < tiles.size(); ++i ) {
+            const auto &tile = tiles[i];
+            SnakeTile Tile;
+            for ( const auto &vertex : tile) {
+                QGeoCoordinate QVertex(vertex[0], vertex[1], vertex[2]);
+                Tile.append(QVertex);
+            }
+            const auto &centerPoint = cps[i];
+            QGeoCoordinate QCenterPoint(centerPoint[0], centerPoint[1], centerPoint[2]);
+            Tile.setCenter(QCenterPoint);
+            _result.tiles.polygons().append(Tile);
+            _result.tileCenterPoints.append(QVariant::fromValue(QCenterPoint));
+         }
+
+        // Get local tiles.
+        const auto &tilesENU = scenario.getTilesENU();
+        for ( unsigned int i=0; i < tilesENU.size(); ++i ) {
+            const auto &tile = tilesENU[i];
+            Polygon2D Tile;
+            for ( const auto &vertex : tile.outer()) {
+                QPointF QVertex(vertex.get<0>(), vertex.get<1>());
+                Tile.path().append(QVertex);
+            }
+            _result.tilesLocal.polygons().append(Tile);
+        }
+
     }
 }
+
+double SnakeWorker::minTransectLength() const
+{
+    return _minTransectLength;
+}
+
+void SnakeWorker::setMinTransectLength(double minTransectLength)
+{
+    if (minTransectLength > 0)
+        _minTransectLength = minTransectLength;
+}
+
+double SnakeWorker::minTileArea() const
+{
+    return _minTileArea;
+}
+
+void SnakeWorker::setLineDistance(double lineDistance)
+{
+    if (lineDistance > 0)
+        _lineDistance = lineDistance;
+}
+
+double SnakeWorker::tileWidth() const
+{
+    return _tileWidth;
+}
+
+void SnakeWorker::setTileWidth(double tileWidth)
+{
+    if (tileWidth > 0)
+        _tileWidth = tileWidth;
+}
+
+double SnakeWorker::tileHeight() const
+{
+    return _tileHeight;
+}
+
+void SnakeWorker::setTileHeight(double tileHeight)
+{
+    if (tileHeight > 0)
+        _tileHeight = tileHeight;
+}
+
+void SnakeWorker::setMinTileArea(double minTileArea)
+{
+    if (minTileArea > 0)
+        _minTileArea = minTileArea;
+}
+
+void Result::clear()
+{
+    this->success = false;
+    this->errorMessage = "";
+    this->waypoints.clear();
+    this->returnPathIdx.clear();
+    this->arrivalPathIdx.clear();
+    this->tiles.polygons().clear();
+    this->tilesLocal.polygons().clear();
+    this->tileCenterPoints.clear();
+}
diff --git a/src/Wima/Snake/SnakeWorker.h b/src/Wima/Snake/SnakeWorker.h
index 06450cec593010525daca9f29cda044c6bc716f6..f7382b758a5295e9b4511113a4e954c8affaa50f 100644
--- a/src/Wima/Snake/SnakeWorker.h
+++ b/src/Wima/Snake/SnakeWorker.h
@@ -4,23 +4,27 @@
 #include <QVariant>
 #include <QThread>
 #include <QVector>
+#include <QGeoCoordinate>
+#include <vector>
 
-#include "snake_geometry.h"
-#include "snake.h"
-
-using namespace snake;
-using namespace snake_geometry;
-using namespace std;
+#include "SnakeTiles.h"
+#include "SnakeTilesLocal.h"
 
 
 typedef QList<QVariant> QVariantList;
 
 typedef struct Result{
-    QVariantList            waypoints;
+    QVector<QGeoCoordinate> waypoints;
+    SnakeTiles              tiles;
+    SnakeTilesLocal         tilesLocal;
+    QVariantList            tileCenterPoints;
+    QGeoCoordinate          origin;
     QVector<unsigned long>  arrivalPathIdx;
     QVector<unsigned long>  returnPathIdx;
     bool                    success;
-    QString                 errorMessage;
+    QString                 errorMessage;    
+
+    void clear();
 }WorkerResult_t;
 
 class SnakeWorker : public QThread{
@@ -30,21 +34,86 @@ public:
     SnakeWorker(QObject *parent = nullptr);
     ~SnakeWorker() override;
 
-    void setScenario            (const Scenario  &scenario);
-    void setProgress            (const QVector<int> &progress);
-    void setLineDistance        (double lineDistance);
-    void setMinTransectLength   (double minTransectLength);
 
-    const WorkerResult_t &getResult() const;
+    template<template<class, class...> class Container>
+    void setMeasurementArea     (const Container<QGeoCoordinate>  &measurementArea);
+    template<template<class, class...> class Container>
+    void setServiceArea         (const Container<QGeoCoordinate>  &serviceArea);
+    template<template<class, class...> class Container>
+    void setCorridor            (const Container<QGeoCoordinate>  &corridor);
+    template<template<class, class...> class Container, class IntType>
+    void setProgress            (const Container<IntType>  &progress);
+
+    WorkerResult_t &result();
+
+    double lineDistance() const;
+    void setLineDistance(double lineDistance);
+
+    double minTransectLength() const;
+    void setMinTransectLength(double minTransectLength);
+
+    double minTileArea() const;
+    void setMinTileArea(double minTileArea);
+
+    double tileHeight() const;
+    void setTileHeight(double tileHeight);
+
+    double tileWidth() const;
+    void setTileWidth(double tileWidth);
 
 protected:
     void run() override;
 
 private:
-    Scenario        _scenario;
-    vector<int8_t>  _progress;
-    double          _lineDistance;
-    double          _minTransectLength;
-    WorkerResult_t  _result;
+    bool precondition() const;
+
+
+    std::vector<QGeoCoordinate> _mArea;
+    std::vector<QGeoCoordinate> _sArea;
+    std::vector<QGeoCoordinate> _corridor;
+    std::vector<int>            _progress;
+    double                      _lineDistance;
+    double                      _minTransectLength;
+    double                      _tileWidth;
+    double                      _tileHeight;
+    double                      _minTileArea;
+    WorkerResult_t              _result;
 };
 
+template<template<class, class...> class Container>
+void SnakeWorker::setCorridor(const Container<QGeoCoordinate> &corridor)
+{
+    _corridor.clear();
+    for (auto vertex : corridor) {
+        _corridor.push_back(vertex);
+    }
+}
+
+template<template<class, class...> class Container>
+void SnakeWorker::setMeasurementArea(const Container<QGeoCoordinate> &measurementArea)
+{
+    _mArea.clear();
+    for (auto vertex : measurementArea) {
+        _mArea.push_back(vertex);
+    }
+}
+
+template<template<class, class...> class Container>
+void SnakeWorker::setServiceArea(const Container<QGeoCoordinate> &serviceArea)
+{
+    _sArea.clear();
+    for (auto vertex : serviceArea) {
+        _sArea.push_back(vertex);
+    }
+}
+
+template<template<class, class...> class Container, class IntType>
+void SnakeWorker::setProgress(const Container<IntType> &progress)
+{
+    _progress.clear();
+    for (auto p : progress) {
+        assert(p >= -1 && p <= 100);
+        _progress.push_back(p);
+    }
+}
+
diff --git a/src/Wima/WaypointManager/Slicer.cpp b/src/Wima/WaypointManager/Slicer.cpp
index a880f52258536244fffc6e362c7344e1d0754dcf..6b13a0e019f1eb579f4edd2800a91054c13af6a3 100644
--- a/src/Wima/WaypointManager/Slicer.cpp
+++ b/src/Wima/WaypointManager/Slicer.cpp
@@ -43,16 +43,15 @@ void Slicer::_updateIdx(long size)
 {
     _overlap = _overlap < _N ? _overlap : _N-1;
 
-    long maxStart  = size-_N;
-    _idxStart = _idxStart <= maxStart   ? _idxStart : maxStart;
-    _idxStart = _idxStart < 0           ? 0         : _idxStart;
+    _idxStart = _idxStart < size   ? _idxStart : size-1;
+    _idxStart = _idxStart < 0      ? 0         : _idxStart;
 
     _idxEnd = _idxStart + _N - 1;
     _idxEnd = _idxEnd < size ? _idxEnd : size-1;
 
-    _idxNext = _idxEnd + 1 - _overlap;
-    _idxNext = _idxNext < 0       ? 0        : _idxNext;
-    _idxNext = _idxNext < size    ? _idxNext : size-1;
+    _idxNext = _idxEnd == size -1 ? _idxStart : _idxEnd + 1 - _overlap;
+    _idxNext = _idxNext < 0       ? 0         : _idxNext;
+    _idxNext = _idxNext < size    ? _idxNext  : size-1;
 
     _idxPrevious = _idxStart + _overlap - _N;
     _idxPrevious = _idxPrevious < 0       ? 0            : _idxPrevious;
diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc
index b3fb20ac580791c9709bc80625b45c13ed4cec86..b2455215104ec0cd531c2ff7237e814202f324c2 100644
--- a/src/Wima/WimaController.cc
+++ b/src/Wima/WimaController.cc
@@ -55,6 +55,7 @@ WimaController::WimaController(QObject *parent)
     , _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])
@@ -68,14 +69,14 @@ WimaController::WimaController(QObject *parent)
     , _measurementPathLength    (-1)
     , _lowBatteryHandlingTriggered  (false)
     , _snakeCalcInProgress      (false)
-    , _scenarioDefinedBool      (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*/)
+    , _fallbackStatus           (0 /*status: not connected*/)
+    , _bridgeSetupDone          (false)
     , _pRosBridge               (new ROSBridge::ROSBridge())
 {
     // Set up facts.
@@ -88,9 +89,20 @@ WimaController::WimaController(QObject *parent)
     connect(&_arrivalReturnSpeed,           &Fact::rawValueChanged, this, &WimaController::_updateArrivalReturnSpeed);
     connect(&_altitude,                     &Fact::rawValueChanged, this, &WimaController::_updateAltitude);
 
-    _defaultManager.setOverlap(_overlapWaypoints.rawValue().toUInt());
-    _defaultManager.setN(_maxWaypointsPerPhase.rawValue().toUInt());
-    _defaultManager.setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt());
+    // Init waypoint managers.
+    bool value;
+    size_t overlap  = _overlapWaypoints.rawValue().toUInt(&value);
+    Q_ASSERT(value);
+    size_t N        = _maxWaypointsPerPhase.rawValue().toUInt(&value);
+    Q_ASSERT(value);
+    size_t startIdx = _nextPhaseStartWaypointIndex.rawValue().toUInt(&value);
+    Q_ASSERT(value);
+    (void)value;
+    for (auto manager : _managerList){
+        manager->setOverlap(overlap);
+        manager->setN(N);
+        manager->setStartIndex(startIdx);
+    }
 
     // Periodic.
     connect(&_eventTimer, &QTimer::timeout, this, &WimaController::_eventTimerHandler);
@@ -103,10 +115,6 @@ WimaController::WimaController(QObject *parent)
     connect(this, &QObject::destroyed, &this->_snakeWorker, &SnakeWorker::quit);
 
     // Snake.
-    connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_startStopRosBridge);
-    _startStopRosBridge();
-    connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_initStartSnakeWorker);
-    _initStartSnakeWorker();
     connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_switchSnakeManager);
     _switchSnakeManager(_enableSnake.rawValue());
 }
@@ -342,13 +350,6 @@ bool WimaController::_calcShortestPath(const QGeoCoordinate &start, const QGeoCo
     return  retVal;
 }
 
-/*!
- * \fn void WimaController::containerDataValidChanged(bool valid)
- * Pulls plan data generated by \c WimaPlaner from the \c _container if the data is valid (\a valid equals true).
- * Is connected to the dataValidChanged() signal of the \c WimaDataContainer.
- *
- * \sa WimaDataContainer, WimaPlaner, WimaPlanData
- */
 bool WimaController::setWimaPlanData(const WimaPlanData &planData)
 {
     // fetch only if valid, return true on success
@@ -446,80 +447,10 @@ bool WimaController::setWimaPlanData(const WimaPlanData &planData)
     emit waypointPathChanged();
     emit currentWaypointPathChanged();
 
+    _localPlanDataValid = true;
 
+    _initStartSnakeWorker();
 
-    // Initialize _scenario.
-    Area mArea;
-    for (auto variant : _measurementArea.path()){
-        QGeoCoordinate c{variant.value<QGeoCoordinate>()};
-        mArea.geoPolygon.push_back(GeoPoint2D{c.latitude(), c.longitude()});
-    }
-    mArea.type = AreaType::MeasurementArea;
-
-    Area sArea;
-    for (auto variant : _serviceArea.path()){
-        QGeoCoordinate c{variant.value<QGeoCoordinate>()};
-        sArea.geoPolygon.push_back(GeoPoint2D{c.latitude(), c.longitude()});
-    }
-    sArea.type = AreaType::ServiceArea;
-
-    Area corridor;
-    for (auto variant : _corridor.path()){
-        QGeoCoordinate c{variant.value<QGeoCoordinate>()};
-        corridor.geoPolygon.push_back(GeoPoint2D{c.latitude(), c.longitude()});
-    }
-    corridor.type = AreaType::Corridor;
-
-    _scenario.addArea(mArea);
-    _scenario.addArea(sArea);
-    _scenario.addArea(corridor);
-
-    // Check if scenario is defined.
-    if ( !_isScenarioDefinedErrorMessage() ) {
-        Q_ASSERT(false);
-        return false;
-    }
-
-    {
-        // Get tiles and origin.
-        auto origin = _scenario.getOrigin();
-        _snakeOrigin.setLatitude(origin[0]);
-        _snakeOrigin.setLongitude(origin[1]);
-        _snakeOrigin.setAltitude(origin[2]);
-        const auto &tiles = _scenario.getTiles();
-        const auto &cps = _scenario.getTileCenterPoints();
-        for ( unsigned int i=0; i < tiles.size(); ++i ) {
-            const auto &tile = tiles[i];
-            SnakeTile Tile;
-            for ( const auto &vertex : tile) {
-                QGeoCoordinate QVertex(vertex[0], vertex[1], vertex[2]);
-                Tile.append(QVertex);
-            }
-            const auto &centerPoint = cps[i];
-            QGeoCoordinate QCenterPoint(centerPoint[0], centerPoint[1], centerPoint[2]);
-            Tile.setCenter(QCenterPoint);
-            _snakeTiles.polygons().append(Tile);
-            _snakeTileCenterPoints.append(QVariant::fromValue(QCenterPoint));
-        }
-    }
-
-    {
-        // Get local tiles.
-        const auto &tiles = _scenario.getTilesENU();
-        for ( unsigned int i=0; i < tiles.size(); ++i ) {
-            const auto &tile = tiles[i];
-            Polygon2D Tile;
-            for ( const auto &vertex : tile.outer()) {
-                QPointF QVertex(vertex.get<0>(), vertex.get<1>());
-                Tile.path().append(QVertex);
-            }
-            _snakeTilesLocal.polygons().append(Tile);
-        }
-    }
-    emit snakeTilesChanged();
-    emit snakeTileCenterPointsChanged();
-
-    _localPlanDataValid = true;
     return true;
 }
 
@@ -692,8 +623,7 @@ void WimaController::_checkBatteryLevel()
 void WimaController::_eventTimerHandler()
 {
     static EventTicker batteryLevelTicker(EVENT_TIMER_INTERVAL, CHECK_BATTERY_INTERVAL);
-    static EventTicker snakeEventLoopTicker(EVENT_TIMER_INTERVAL, SNAKE_EVENT_LOOP_INTERVAL);
-    static EventTicker rosBridgeTicker(EVENT_TIMER_INTERVAL, 1000);
+    static EventTicker snakeTicker(EVENT_TIMER_INTERVAL, SNAKE_EVENT_LOOP_INTERVAL);
     static EventTicker nemoStatusTicker(EVENT_TIMER_INTERVAL, 5000);
 
     // Battery level check necessary?
@@ -713,29 +643,72 @@ void WimaController::_eventTimerHandler()
         emit WimaController::nemoStatusStringChanged();
     }
 
-    if (_enableSnake.rawValue().toBool() && rosBridgeTicker.ready()) {
-
-        _pRosBridge->publish(_snakeTilesLocal, "/snake/tiles");
-        _pRosBridge->publish(_snakeOrigin, "/snake/origin");
-
-        using namespace std::placeholders;
-        auto progressCallBack = std::bind(&WimaController::_progressFromJson,
-                                          this,
-                                          _1,
-                                          std::ref(_nemoProgress));
-        _pRosBridge->subscribe("/nemo/progress", progressCallBack);
-        _pRosBridge->subscribe("/nemo/heartbeat", [this, &nemoStatusTicker](JsonDocUPtr pDoc){
-            if ( !this->_pRosBridge->casePacker()->unpack(pDoc, this->_nemoHeartbeat) ) {
-                if ( this->_nemoHeartbeat.status() == this->_fallbackStatus )
-                    return;
-                this->_nemoHeartbeat.setStatus(this->_fallbackStatus);
+    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;
             }
-
-            nemoStatusTicker.reset();
-            this->_fallbackStatus = -1; /*Timeout*/
-            emit WimaController::nemoStatusChanged();
-            emit WimaController::nemoStatusStringChanged();
-        });
+        } else if ( _bridgeSetupDone) {
+            _pRosBridge->reset();
+            _bridgeSetupDone = false;
+        }
     }
 }
 
@@ -784,14 +757,23 @@ void WimaController::_switchWaypointManager(WaypointManager::ManagerBase &manage
     if (_currentManager != &manager) {
         _currentManager = &manager;
 
-        bool value;
-        _currentManager->setN(_maxWaypointsPerPhase.rawValue().toUInt(&value));
-        Q_ASSERT(value);
-        _currentManager->setOverlap(_overlapWaypoints.rawValue().toUInt(&value));
-        Q_ASSERT(value);
-        _currentManager->setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt(&value)-1);
-        Q_ASSERT(value);
-        (void)value;
+        disconnect(&_overlapWaypoints,             &Fact::rawValueChanged,
+                   this, &WimaController::_updateOverlap);
+        disconnect(&_maxWaypointsPerPhase,         &Fact::rawValueChanged,
+                   this, &WimaController::_updateMaxWaypoints);
+        disconnect(&_nextPhaseStartWaypointIndex,  &Fact::rawValueChanged,
+                   this, &WimaController::_setStartIndex);
+
+        _maxWaypointsPerPhase.setRawValue(_currentManager->N());
+        _overlapWaypoints.setRawValue(_currentManager->overlap());
+        _nextPhaseStartWaypointIndex.setRawValue(_currentManager->startIndex());
+
+        connect(&_overlapWaypoints,             &Fact::rawValueChanged,
+                this, &WimaController::_updateOverlap);
+        connect(&_maxWaypointsPerPhase,         &Fact::rawValueChanged,
+                this, &WimaController::_updateMaxWaypoints);
+        connect(&_nextPhaseStartWaypointIndex,  &Fact::rawValueChanged,
+                this, &WimaController::_setStartIndex);
 
         emit missionItemsChanged();
         emit currentMissionItemsChanged();
@@ -834,35 +816,15 @@ void WimaController::_setSnakeCalcInProgress(bool inProgress)
     }
 }
 
-bool WimaController::_isScenarioDefined()
-{
-    _scenarioDefinedBool = _scenario.defined(_snakeTileWidth.rawValue().toDouble(),
-                                             _snakeTileHeight.rawValue().toDouble(),
-                                             _snakeMinTileArea.rawValue().toDouble());
-    return _scenarioDefinedBool;
-}
-
-bool WimaController::_isScenarioDefinedErrorMessage()
-{
-    bool value = _isScenarioDefined();
-    if (!value){
-        QString errorString;
-        for (auto c : _scenario.errorString)
-            errorString.push_back(c);
-        qgcApp()->showMessage(errorString);
-    }
-    return value;
-}
-
 void WimaController::_snakeStoreWorkerResults()
 {
     auto start = std::chrono::high_resolution_clock::now();
     _snakeManager.clear();
 
-    const WorkerResult_t &r = _snakeWorker.getResult();
+    const WorkerResult_t &r = _snakeWorker.result();
     _setSnakeCalcInProgress(false);
     if (!r.success) {
-        qgcApp()->showMessage(r.errorMessage);
+        //qgcApp()->showMessage(r.errorMessage);
         return;
     }
 
@@ -876,7 +838,7 @@ void WimaController::_snakeStoreWorkerResults()
     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)].value<QGeoCoordinate>());
+        _snakeManager.push_back(r.waypoints[int(i)]);
     }
 
     _snakeManager.update();
@@ -891,15 +853,6 @@ void WimaController::_snakeStoreWorkerResults()
     qWarning() << "WimaController::_snakeStoreWorkerResults execution time: " << duration << " ms.";
 }
 
-void WimaController::_startStopRosBridge()
-{
-    if ( _enableSnake.rawValue().toBool() ) {
-        _pRosBridge->start();
-    } else {
-        _pRosBridge->reset();
-    }
-}
-
 void WimaController::_initStartSnakeWorker()
 {
     if ( !_enableSnake.rawValue().toBool() )
@@ -911,10 +864,15 @@ void WimaController::_initStartSnakeWorker()
     }
 
     // Initialize _snakeWorker.
-    _snakeWorker.setScenario(_scenario);
+    _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.
@@ -929,16 +887,3 @@ void WimaController::_switchSnakeManager(QVariant variant)
         _switchWaypointManager(_defaultManager);
     }
 }
-
-void WimaController::_progressFromJson(JsonDocUPtr pDoc,
-                                       QNemoProgress &progress)
-{
-    int requiredSize = _snakeTilesLocal.polygons().size();
-    if ( !_pRosBridge->casePacker()->unpack(pDoc, progress)
-         || progress.progress().size() != requiredSize ) {
-        progress.progress().fill(0, requiredSize);
-    }
-
-    emit WimaController::nemoProgressChanged();
-}
-
diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h
index 60536124a5fde5005ee32ea598581741e2b5fda8..edf33e85c425d8687d5156f6a5fbebb5355eba99 100644
--- a/src/Wima/WimaController.h
+++ b/src/Wima/WimaController.h
@@ -46,7 +46,7 @@
 #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 5000 // ms
+#define SNAKE_EVENT_LOOP_INTERVAL 1000 // ms
 
 
 using namespace snake;
@@ -338,10 +338,7 @@ private slots:
     void    _smartRTLCleanUp                    (bool flying);
     // Snake.
     void _setSnakeCalcInProgress             (bool inProgress);
-    bool _isScenarioDefined                  (void);
-    bool _isScenarioDefinedErrorMessage      (void);
     void _snakeStoreWorkerResults            ();
-    void _startStopRosBridge                 ();
     void _initStartSnakeWorker               ();
     void _switchSnakeManager                 (QVariant variant);
     // Periodic tasks.
@@ -351,16 +348,12 @@ private slots:
 
 private:
     using StatusMap = std::map<int, QString>;
-    // Snake.
-    void _progressFromJson                   (JsonDocUPtr pDoc,
-                                              QNemoProgress &progress);
 
     // 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
@@ -375,6 +368,8 @@ private:
     WaypointManager::DefaultManager     _snakeManager;
     WaypointManager::RTLManager         _rtlManager;
     WaypointManager::ManagerBase       *_currentManager;
+    using ManagerList = QList<WaypointManager::ManagerBase*>;
+    ManagerList                         _managerList;
 
     // Settings Facts.
     QMap<QString, FactMetaData*> _metaDataMap;
@@ -406,11 +401,8 @@ private:
 
     // Snake
     bool                    _snakeCalcInProgress;
-    bool                    _snakeRecalcNecessary;
-    bool                    _scenarioDefinedBool;
     SnakeWorker             _snakeWorker;
-    Scenario                _scenario;
-    ::GeoPoint3D            _snakeOrigin;
+    GeoPoint              _snakeOrigin;
     SnakeTiles              _snakeTiles; // tiles
     SnakeTilesLocal         _snakeTilesLocal; // tiles local coordinate system
     QVariantList            _snakeTileCenterPoints;
@@ -418,6 +410,7 @@ private:
     QNemoHeartbeat          _nemoHeartbeat; // measurement progress
     int                     _fallbackStatus;
     ROSBridgePtr            _pRosBridge;
+    bool                    _bridgeSetupDone;
     static StatusMap        _nemoStatusMap;
 
     // Periodic tasks.
diff --git a/src/comm/ros_bridge/include/ComPrivateInclude.h b/src/comm/ros_bridge/include/ComPrivateInclude.h
index c83cdb97406f4214051835cada29afd392ff07d1..d6285509917704fb5434b70ab38b13c5d58abc82 100644
--- a/src/comm/ros_bridge/include/ComPrivateInclude.h
+++ b/src/comm/ros_bridge/include/ComPrivateInclude.h
@@ -5,7 +5,7 @@
 
 #include <memory>
 #include <deque>
-#include <set>
+#include <unordered_map>
 
 namespace ROSBridge {
 namespace ComPrivate {
@@ -15,7 +15,8 @@ typedef rapidjson::Document JsonDoc;
 typedef std::unique_ptr<JsonDoc> JsonDocUPtr;
 typedef std::deque<JsonDocUPtr> JsonQueue;
 typedef std::size_t HashType;
-typedef std::set<HashType> HashSet;
+
+using ClientMap = std::unordered_map<HashType, std::string>;
 
 static const char* _topicAdvertiserKey = "topic_advertiser";
 static const char* _topicPublisherKey =  "topic_publisher";
diff --git a/src/comm/ros_bridge/include/MessageGroups.h b/src/comm/ros_bridge/include/MessageGroups.h
index 8572183e1e3a9cbd039d2ddadc8d1a79c0b8cd29..7b95eef76aa215455731100b6007595b588266bb 100644
--- a/src/comm/ros_bridge/include/MessageGroups.h
+++ b/src/comm/ros_bridge/include/MessageGroups.h
@@ -79,6 +79,17 @@ struct GeometryMsgs {
     };
 };
 
+struct GeographicMsgs {
+
+    static StringType label() {return "geographic_msgs";}
+
+    //!
+    //! \brief The GeoPointGroup struct is used the mark a class as a ROS geographic_msgs/GeoPoint message.
+    struct GeoPointGroup {
+        static StringType label() {return "GeoPoint";}
+    };
+};
+
 struct JSKRecognitionMsgs {
 
     static StringType label() {return "jsk_recognition_msgs";}
@@ -118,8 +129,8 @@ typedef MessageGroups::MessageGroup<MessageGroups::GeometryMsgs,
                                     MessageGroups::GeometryMsgs::Point32Group>
         Point32Group;
 
-typedef MessageGroups::MessageGroup<MessageGroups::GeometryMsgs,
-                                    MessageGroups::GeometryMsgs::GeoPointGroup>
+typedef MessageGroups::MessageGroup<MessageGroups::GeographicMsgs,
+                                    MessageGroups::GeographicMsgs::GeoPointGroup>
         GeoPointGroup;
 
 typedef MessageGroups::MessageGroup<MessageGroups::GeometryMsgs,
diff --git a/src/comm/ros_bridge/include/ROSBridge.h b/src/comm/ros_bridge/include/ROSBridge.h
index 48b090502c2e08ab0c90c6d1a91b2cb7a3dc13ec..6ee6f94e9e15cb2969ab876b87b53a82c29fd199 100644
--- a/src/comm/ros_bridge/include/ROSBridge.h
+++ b/src/comm/ros_bridge/include/ROSBridge.h
@@ -34,8 +34,11 @@ public:
 
     //! @brief Start ROS bridge.
     void start();
-    //! @brief Reset ROS bridge.
+    //! @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.
+    bool connected();
 
 private:
     TypeFactory             _typeFactory;
diff --git a/src/comm/ros_bridge/include/RosBridgeClient.h b/src/comm/ros_bridge/include/RosBridgeClient.h
index 76805dfc0ae961ebeb66032eaccc54a69e293698..d2e70ae8c8a640daa857612f0a221b9e0bf432f3 100644
--- a/src/comm/ros_bridge/include/RosBridgeClient.h
+++ b/src/comm/ros_bridge/include/RosBridgeClient.h
@@ -15,6 +15,9 @@
 #include <chrono>
 #include <functional>
 #include <thread>
+#include <future>
+#include <functional>
+#include <mutex>
 
 using WsClient = SimpleWeb::SocketClient<SimpleWeb::WS>;
 using InMessage =  std::function<void(std::shared_ptr<WsClient::Connection>, std::shared_ptr<WsClient::InMessage>)>;
@@ -23,18 +26,20 @@ class RosbridgeWsClient
 {
  std::string server_port_path;
  std::unordered_map<std::string, std::shared_ptr<WsClient>> client_map;
+ std::mutex map_mutex;
 
+ // Starts the client.
  void start(const std::string& client_name, std::shared_ptr<WsClient> client, const std::string& message)
  {
    if (!client->on_open)
    {
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_CLIENT_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_CLIENT_DEBUG
        std::cout << client_name << ": Opened connection" << std::endl;
        std::cout << client_name << ": Sending message: " << message << std::endl;
 #endif
@@ -42,7 +47,7 @@ class RosbridgeWsClient
      };
    }
 
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_CLIENT_DEBUG
    if (!client->on_message)
    {
      client->on_message = [client_name](std::shared_ptr<WsClient::Connection> /*connection*/, std::shared_ptr<WsClient::InMessage> in_message) {
@@ -66,14 +71,14 @@ class RosbridgeWsClient
    }
 #endif
 
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_CLIENT_DEBUG
    std::thread client_thread([client_name, client]() {
 #else
    std::thread client_thread([client]() {
 #endif
      client->start();
 
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_CLIENT_DEBUG
      std::cout << client_name << ": Terminated" << std::endl;
 #endif
      client->on_open = NULL;
@@ -83,9 +88,6 @@ class RosbridgeWsClient
    });
 
    client_thread.detach();
-
-   // This is to make sure that the thread got fully launched before we do anything to it (e.g. remove)
-   std::this_thread::sleep_for(std::chrono::milliseconds(100));
  }
 
 public:
@@ -96,6 +98,8 @@ public:
 
  ~RosbridgeWsClient()
  {
+
+   std::lock_guard<std::mutex> lk(map_mutex); // neccessary?
    for (auto& client : client_map)
    {
      client.second->stop();
@@ -103,14 +107,44 @@ public:
    }
  }
 
+ // The execution can take up to 100 ms!
+ bool connected(){
+
+    auto p = std::make_shared<std::promise<void>>();
+    auto future = p->get_future();
+
+    auto callback = [](std::shared_ptr<WsClient::Connection> connection, std::shared_ptr<std::promise<void>> p) {
+        p->set_value();
+        connection->send_close(1000);
+    };
+    std::shared_ptr<WsClient> status_client = std::make_shared<WsClient>(server_port_path);
+    status_client->on_open = std::bind(callback, std::placeholders::_1, p);
+
+    std::async([status_client]{
+
+        status_client->start();
+        status_client->on_open = NULL;
+        status_client->on_message = NULL;
+        status_client->on_close = NULL;
+        status_client->on_error = NULL;
+
+    });
+
+    auto status = future.wait_for(std::chrono::milliseconds(100));
+    return status == std::future_status::ready;
+ }
+
+ // Adds a client to the client_map.
  void addClient(const std::string& client_name)
  {
+
+   std::lock_guard<std::mutex> lk(map_mutex);
    std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
    if (it == client_map.end())
    {
      client_map[client_name] = std::make_shared<WsClient>(server_port_path);
    }
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_CLIENT_DEBUG
    else
    {
      std::cerr << client_name << " has already been created" << std::endl;
@@ -120,6 +154,8 @@ public:
 
  std::shared_ptr<WsClient> getClient(const std::string& client_name)
  {
+
+   std::lock_guard<std::mutex> lk(map_mutex);
    std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
    if (it != client_map.end())
    {
@@ -130,6 +166,8 @@ public:
 
  void stopClient(const std::string& client_name)
  {
+
+   std::lock_guard<std::mutex> lk(map_mutex);
    std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
    if (it != client_map.end())
    {
@@ -138,11 +176,11 @@ public:
      it->second->on_message = NULL;
      it->second->on_close = NULL;
      it->second->on_error = NULL;
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_CLIENT_DEBUG
      std::cout << client_name << " has been stopped" << std::endl;
 #endif
    }
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_CLIENT_DEBUG
    else
    {
      std::cerr << client_name << " has not been created" << std::endl;
@@ -152,17 +190,27 @@ public:
 
  void removeClient(const std::string& client_name)
  {
+
+   std::lock_guard<std::mutex> lk(map_mutex);
    std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
    if (it != client_map.end())
    {
-     it->second->stop();
-     it->second.reset();
+     // Stop the client asynchronously in 100 ms.
+     // This is to ensure, that all threads involving client have been launched.
+     std::thread t([](std::shared_ptr<WsClient> client){
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        client->stop();
+        client.reset();
+     },
+     it->second /*lambda param*/ );
+
      client_map.erase(it);
-#ifdef DEBUG
+     t.detach();
+#ifdef ROS_BRIDGE_CLIENT_DEBUG
      std::cout << client_name << " has been removed" << std::endl;
 #endif
    }
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_CLIENT_DEBUG
    else
    {
      std::cerr << client_name << " has not been created" << std::endl;
@@ -170,8 +218,12 @@ public:
 #endif
  }
 
+ // Gets the client from client_map and starts it. Advertising is essentially sending a message.
+ // One client per topic!
  void advertise(const std::string& client_name, const std::string& topic, const std::string& type, const std::string& id = "")
  {
+
+   std::lock_guard<std::mutex> lk(map_mutex);
    std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
    if (it != client_map.end())
    {
@@ -185,7 +237,7 @@ public:
 
      start(client_name, it->second, message);
    }
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_CLIENT_DEBUG
    else
    {
      std::cerr << client_name << "has not been created" << std::endl;
@@ -210,7 +262,7 @@ public:
    std::shared_ptr<WsClient> publish_client = std::make_shared<WsClient>(server_port_path);
 
    publish_client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_CLIENT_DEBUG
      std::cout << "publish_client: Opened connection" << std::endl;
      std::cout << "publish_client: Sending message: " << message << std::endl;
 #endif
@@ -225,6 +277,8 @@ public:
 
  void subscribe(const std::string& client_name, const std::string& topic, const InMessage& callback, const std::string& id = "", const std::string& type = "", int throttle_rate = -1, int queue_length = -1, int fragment_size = -1, const std::string& compression = "")
  {
+
+   std::lock_guard<std::mutex> lk(map_mutex);
    std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
    if (it != client_map.end())
    {
@@ -259,7 +313,7 @@ public:
      it->second->on_message = callback;
      start(client_name, it->second, message);
    }
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_CLIENT_DEBUG
    else
    {
      std::cerr << client_name << "has not been created" << std::endl;
@@ -269,6 +323,8 @@ public:
 
  void advertiseService(const std::string& client_name, const std::string& service, const std::string& type, const InMessage& callback)
  {
+
+   std::lock_guard<std::mutex> lk(map_mutex);
    std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
    if (it != client_map.end())
    {
@@ -277,7 +333,7 @@ public:
      it->second->on_message = callback;
      start(client_name, it->second, message);
    }
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_CLIENT_DEBUG
    else
    {
      std::cerr << client_name << "has not been created" << std::endl;
@@ -304,7 +360,7 @@ public:
    std::shared_ptr<WsClient> service_response_client = std::make_shared<WsClient>(server_port_path);
 
    service_response_client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_CLIENT_DEBUG
      std::cout << "service_response_client: Opened connection" << std::endl;
      std::cout << "service_response_client: Sending message: " << message << std::endl;
 #endif
@@ -351,7 +407,7 @@ public:
    else
    {
      call_service_client->on_message = [](std::shared_ptr<WsClient::Connection> connection, std::shared_ptr<WsClient::InMessage> in_message) {
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_CLIENT_DEBUG
        std::cout << "call_service_client: Message received: " << in_message->string() << std::endl;
        std::cout << "call_service_client: Sending close connection" << std::endl;
 #endif
diff --git a/src/comm/ros_bridge/include/TopicPublisher.cpp b/src/comm/ros_bridge/include/TopicPublisher.cpp
index 3b8d62639e94466610fdcf6fdadef5cdeb7cabdb..1e44a7a74804f465ca7f2f9365dc450bb26d664e 100644
--- a/src/comm/ros_bridge/include/TopicPublisher.cpp
+++ b/src/comm/ros_bridge/include/TopicPublisher.cpp
@@ -2,8 +2,20 @@
 
 
 
-ROSBridge::ComPrivate::TopicPublisher::TopicPublisher(CasePacker *casePacker,
-                                                      RosbridgeWsClient *rbc) :
+
+
+struct ROSBridge::ComPrivate::ThreadData
+{
+    const ROSBridge::CasePacker      &casePacker;
+    RosbridgeWsClient                &rbc;
+    ROSBridge::ComPrivate::JsonQueue &queue;
+    std::mutex                       &queueMutex;
+    const std::atomic<bool>          &running;
+    std::condition_variable          &cv;
+};
+
+ROSBridge::ComPrivate::TopicPublisher::TopicPublisher(CasePacker &casePacker,
+                                                      RosbridgeWsClient &rbc) :
       _running(false)
     , _casePacker(casePacker)
     , _rbc(rbc)
@@ -21,14 +33,15 @@ void ROSBridge::ComPrivate::TopicPublisher::start()
     if ( _running.load() )  // start called while thread running.
         return;
     _running.store(true);
-    _rbc->addClient(ROSBridge::ComPrivate::_topicAdvertiserKey);
-    _pThread.reset(new std::thread(&ROSBridge::ComPrivate::transmittLoop,
-                                   std::cref(*_casePacker),
-                                   std::ref(*_rbc),
-                                   std::ref(_queue),
-                                   std::ref(_queueMutex),
-                                   std::ref(_advertisedTopicsHashList),
-                                   std::cref(_running)));
+    ROSBridge::ComPrivate::ThreadData data{
+                _casePacker,
+                _rbc,
+                _queue,
+                _queueMutex,
+                _running,
+                _cv
+    };
+    _pThread = std::make_unique<std::thread>(&ROSBridge::ComPrivate::transmittLoop, data);
 }
 
 void ROSBridge::ComPrivate::TopicPublisher::reset()
@@ -36,66 +49,61 @@ void ROSBridge::ComPrivate::TopicPublisher::reset()
     if ( !_running.load() )  // stop called while thread not running.
         return;
     _running.store(false);
+    _cv.notify_one(); // Wake publisher thread.
     if ( !_pThread )
         return;
     _pThread->join();
-    _pThread.reset();    
-    _rbc->removeClient(ROSBridge::ComPrivate::_topicAdvertiserKey);
     _queue.clear();
-    _advertisedTopicsHashList.clear();
 }
 
-void ROSBridge::ComPrivate::transmittLoop(const ROSBridge::CasePacker &casePacker,
-                                          RosbridgeWsClient &rbc,
-                                          ROSBridge::ComPrivate::JsonQueue &queue,
-                                          std::mutex &queueMutex,
-                                          HashSet &advertisedTopicsHashList,
-                                          const std::atomic<bool> &running)
+void ROSBridge::ComPrivate::transmittLoop(ROSBridge::ComPrivate::ThreadData data)
 {
-    rbc.addClient(ROSBridge::ComPrivate::_topicPublisherKey);
-
-    while(running.load()){
-        // Pop message from queue.
-        queueMutex.lock();
-        //std::cout << "Queue size: " << queue.size() << std::endl;
-        if (queue.empty()){
-            queueMutex.unlock();
-            std::this_thread::sleep_for(std::chrono::milliseconds(20));
+    // Init.
+    ClientMap           clientMap;
+    // Main Loop.
+    while(data.running.load()){
+        std::unique_lock<std::mutex> lk(data.queueMutex);
+        // Check if new data available, wait if not.
+        if (data.queue.empty()){
+            data.cv.wait(lk); // Wait for condition, spurious wakeups don't matter in this case.
             continue;
         }
-        JsonDocUPtr pJsonDoc(std::move(queue.front()));
-        queue.pop_front();
-        queueMutex.unlock();
-
-        // Debug output.
-//        std::cout << "Transmitter loop json document:" << std::endl;
-//        rapidjson::OStreamWrapper out(std::cout);
-//        rapidjson::Writer<rapidjson::OStreamWrapper> writer(out);
-//        pJsonDoc->Accept(writer);
-//        std::cout << std::endl << std::endl;
+        // Pop message from queue.
+        JsonDocUPtr pJsonDoc(std::move(data.queue.front()));
+        data.queue.pop_front();
+        lk.unlock();
 
         // Get tag from Json message and remove it.
         Tag tag;
-        bool ret = casePacker.getTag(pJsonDoc, tag);
+        bool ret = data.casePacker.getTag(pJsonDoc, tag);
         assert(ret); // Json message does not contain a tag;
         (void)ret;
-        casePacker.removeTag(pJsonDoc);
+        data.casePacker.removeTag(pJsonDoc);
 
         // Check if topic must be advertised.
         // Advertised topics are stored in advertisedTopicsHashList as
         // a hash.
-        HashType hash = ROSBridge::ComPrivate::getHash(tag.topic());
-        if ( advertisedTopicsHashList.count(hash) == 0) {
-            advertisedTopicsHashList.insert(hash);
-            rbc.advertise(ROSBridge::ComPrivate::_topicAdvertiserKey,
-                          tag.topic(),
-                          tag.messageType() );
+        std::string clientName = ROSBridge::ComPrivate::_topicAdvertiserKey
+                               + tag.topic();
+        HashType hash = ROSBridge::ComPrivate::getHash(clientName);
+        auto it = clientMap.find(hash);
+        if ( it == clientMap.end()) { // Need to advertise topic?
+            clientMap.insert(std::make_pair(hash, clientName));
+            std::cout << clientName << ";"
+                      << tag.topic() << ";"
+                      << tag.messageType() << ";" << std::endl;
+            data.rbc.addClient(clientName);
+            data.rbc.advertise(clientName,
+                               tag.topic(),
+                               tag.messageType() );
         }
-        // Debug output.
-        //std::cout << "Hash Set size: " << advertisedTopicsHashList.size() << std::endl;
 
-        // Send Json message.
-        rbc.publish(tag.topic(), *pJsonDoc.get());
+        // Publish Json message.
+        data.rbc.publish(tag.topic(), *pJsonDoc.get());
     } // while loop
+
+    // Tidy up.
+    for (auto& it : clientMap)
+        data.rbc.removeClient(it.second);
 }
 
diff --git a/src/comm/ros_bridge/include/TopicPublisher.h b/src/comm/ros_bridge/include/TopicPublisher.h
index a762eafd42e01197d9431929ff28ecb29ebb96ba..1c9dc680aec09f50ba5d9a64c3185e7d00ec6b07 100644
--- a/src/comm/ros_bridge/include/TopicPublisher.h
+++ b/src/comm/ros_bridge/include/TopicPublisher.h
@@ -10,18 +10,22 @@
 #include <atomic>
 #include <mutex>
 #include <set>
+#include <condition_variable>
 
 namespace ROSBridge {
 namespace ComPrivate {
 
+struct ThreadData;
+
 class TopicPublisher
 {
     typedef std::unique_ptr<std::thread> ThreadPtr;
+    using CondVar = std::condition_variable;
 public:
 
     TopicPublisher() = delete;
-    TopicPublisher(CasePacker *casePacker,
-                   RosbridgeWsClient *rbc);
+    TopicPublisher(CasePacker &casePacker,
+                   RosbridgeWsClient &rbc);
 
     ~TopicPublisher();
 
@@ -31,34 +35,32 @@ public:
     //! @brief Resets the publisher.
     void reset();
 
-    template<class T>
-    void publish(const T &msg, const std::string &topic){
-        JsonDocUPtr docPtr(_casePacker->pack(msg, topic));
-        publish(std::move(docPtr));
-    }
     void publish(JsonDocUPtr docPtr){
+        {
         std::lock_guard<std::mutex> lock(_queueMutex);
         _queue.push_back(std::move(docPtr));
+        }
+        _cv.notify_one();  // Wake publisher thread.
+    }
+
+    template<class T>
+    void publish(const T &msg, const std::string &topic){
+        JsonDocUPtr docPtr(_casePacker.pack(msg, topic));
+        publish(std::move(docPtr));
     }
 
 private:
     JsonQueue           _queue;
     std::mutex          _queueMutex;
     std::atomic<bool>   _running;
-    CasePacker         *_casePacker;
+    CasePacker         &_casePacker;
+    RosbridgeWsClient  &_rbc;
+    CondVar             _cv;
     ThreadPtr           _pThread;
-    RosbridgeWsClient  *_rbc;
-    HashSet            _advertisedTopicsHashList; // Not thread save! This container
-                                                  // is manipulated by transmittLoop only!
 };
 
 
-void transmittLoop(const ROSBridge::CasePacker &casePacker,
-                   RosbridgeWsClient &rbc,
-                   ROSBridge::ComPrivate::JsonQueue &queue,
-                   std::mutex &queueMutex,
-                   HashSet &advertisedTopicsHashList,
-                   const std::atomic<bool> &running);
+void transmittLoop(ThreadData data);
 
 
 } // namespace CommunicatorDetail
diff --git a/src/comm/ros_bridge/include/TopicSubscriber.cpp b/src/comm/ros_bridge/include/TopicSubscriber.cpp
index a76971253cf90b17be7f03919a9522b11be67e15..7a1d8311fbdfa364ec6bf22c4413ed075d1d2aa8 100644
--- a/src/comm/ros_bridge/include/TopicSubscriber.cpp
+++ b/src/comm/ros_bridge/include/TopicSubscriber.cpp
@@ -1,9 +1,8 @@
 #include "TopicSubscriber.h"
 
 
-ROSBridge::ComPrivate::TopicSubscriber::TopicSubscriber(
-        ROSBridge::CasePacker *casePacker,
-        RosbridgeWsClient *rbc) :
+ROSBridge::ComPrivate::TopicSubscriber::TopicSubscriber(CasePacker &casePacker,
+        RosbridgeWsClient &rbc) :
     _casePacker(casePacker)
   , _rbc(rbc)
   , _running(false)
@@ -24,10 +23,16 @@ void ROSBridge::ComPrivate::TopicSubscriber::start()
 void ROSBridge::ComPrivate::TopicSubscriber::reset()
 {
     if ( _running ){
-        for (std::string clientName : _clientList)
-            _rbc->removeClient(clientName);
         _running = false;
-        _callbackMap.clear();
+        {
+            for (std::string clientName : _clientList){
+                _rbc.removeClient(clientName);
+            }
+        }
+        {
+            std::lock_guard<std::mutex> lk(_callbackMapStruct.mutex);
+            _callbackMapStruct.map.clear();
+        }
         _clientList.clear();
     }
 }
@@ -44,25 +49,39 @@ bool ROSBridge::ComPrivate::TopicSubscriber::subscribe(
     _clientList.push_back(clientName);
 
     HashType hash = getHash(clientName);
-    auto ret = _callbackMap.insert(std::make_pair(hash, callback)); //
-    if ( !ret.second )
-        return false; // Topic subscription already present.
-
     {
+        std::lock_guard<std::mutex> lk(_callbackMapStruct.mutex);
+        auto ret = _callbackMapStruct.map.insert(std::make_pair(hash, callback)); //
+        if ( !ret.second )
+            return false; // Topic subscription already present.
+
+    }
+
     using namespace std::placeholders;
     auto f = std::bind(&ROSBridge::ComPrivate::subscriberCallback,
                       hash,
-                      std::cref(_callbackMap),
+                      std::ref(_callbackMapStruct),
                       _1, _2);
 
 //    std::cout << std::endl;
 //    std::cout << "topic subscription" << std::endl;
 //    std::cout << "client name: " << clientName << std::endl;
 //    std::cout << "topic: " << topic << std::endl;
-    _rbc->addClient(clientName);
-    _rbc->subscribe(clientName,
-                    topic,
-                    f );
+    {
+        auto start = std::chrono::high_resolution_clock::now();
+        _rbc.addClient(clientName);
+        auto end = std::chrono::high_resolution_clock::now();
+        std::cout  << "add client time: "
+                   << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
+                   << " ms" << std::endl;
+        start = std::chrono::high_resolution_clock::now();
+        _rbc.subscribe(clientName,
+                        topic,
+                        f );
+        end = std::chrono::high_resolution_clock::now();
+        std::cout  << "subscribe time: "
+                   << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
+                   << " ms" << std::endl;
     }
 
     return true;
@@ -72,7 +91,7 @@ bool ROSBridge::ComPrivate::TopicSubscriber::subscribe(
 using WsClient = SimpleWeb::SocketClient<SimpleWeb::WS>;
 void ROSBridge::ComPrivate::subscriberCallback(
         const HashType &hash,
-        const ROSBridge::ComPrivate::TopicSubscriber::CallbackMap &map,
+        ROSBridge::ComPrivate::MapStruct &mapWrapper,
         std::shared_ptr<WsClient::Connection>,
         std::shared_ptr<WsClient::InMessage> in_message)
 {
@@ -97,26 +116,21 @@ void ROSBridge::ComPrivate::subscriberCallback(
 //              << std::endl;
 
     // Search callback.
-    auto it = map.find(hash);
-    if (it == map.end()) {
-        assert(false); // callback not found
-        return;
+    CallbackType callback;
+    {
+        std::lock_guard<std::mutex> lk(mapWrapper.mutex);
+        auto it = mapWrapper.map.find(hash);
+        if (it == mapWrapper.map.end()) {
+            //assert(false); // callback not found
+            return;
+        }
+        callback = it->second;
     }
 
     // Extract message and call callback.
     JsonDocUPtr pDoc(new JsonDoc());
     pDoc->CopyFrom(docFull["msg"].Move(), docFull.GetAllocator());
-    it->second(std::move(pDoc)); // Call callback.
+    callback(std::move(pDoc));
     return;
 
 }
-
-void ROSBridge::ComPrivate::test(
-        std::shared_ptr<WsClient::Connection>,
-        std::shared_ptr<WsClient::InMessage> in_message)
-{
-
-    std::cout << "test: Json document: "
-              << in_message->string()
-              << std::endl;
-}
diff --git a/src/comm/ros_bridge/include/TopicSubscriber.h b/src/comm/ros_bridge/include/TopicSubscriber.h
index 3c05edf7a3ba1e48c48a6d35abea63eef588a9bc..585e364771181201657952739438a5fea7b9653b 100644
--- a/src/comm/ros_bridge/include/TopicSubscriber.h
+++ b/src/comm/ros_bridge/include/TopicSubscriber.h
@@ -8,15 +8,21 @@
 namespace ROSBridge {
 namespace ComPrivate {
 
+
+typedef std::function<void(JsonDocUPtr)> CallbackType;
+struct MapStruct{
+    typedef std::map<HashType, CallbackType> Map;
+    Map map;
+    std::mutex mutex;
+};
+
 class TopicSubscriber
 {
     typedef std::vector<std::string> ClientList;
-public:    
-    typedef std::function<void(JsonDocUPtr)> CallbackType;
-    typedef std::map<HashType, CallbackType> CallbackMap;
+public:
 
     TopicSubscriber() = delete;
-    TopicSubscriber(CasePacker *casePacker, RosbridgeWsClient *rbc);
+    TopicSubscriber(CasePacker &casePacker, RosbridgeWsClient &rbc);
     ~TopicSubscriber();
 
     //! @brief Starts the subscriber.
@@ -31,19 +37,19 @@ public:
     bool subscribe(const char* topic, const CallbackType &callback);
 
 private:
-    CasePacker         *_casePacker;
-    RosbridgeWsClient  *_rbc;
-    CallbackMap         _callbackMap;
+
+
+
+    CasePacker         &_casePacker;
+    RosbridgeWsClient  &_rbc;
+    MapStruct           _callbackMapStruct;
     ClientList          _clientList;
     bool                _running;
 };
 
 void subscriberCallback(const HashType &hash,
-                        const TopicSubscriber::CallbackMap &map,
+                        MapStruct &mapWrapper,
                         std::shared_ptr<WsClient::Connection> /*connection*/,
                         std::shared_ptr<WsClient::InMessage> in_message);
-
-void test(std::shared_ptr<WsClient::Connection> /*connection*/,
-                        std::shared_ptr<WsClient::InMessage> in_message);
 } // namespace ComPrivate
 } // namespace ROSBridge
diff --git a/src/comm/ros_bridge/src/ROSBridge.cpp b/src/comm/ros_bridge/src/ROSBridge.cpp
index f18a1da6235242f8544a4f3ad6d54f38a70df619..32d8ab791301c465a0bde1fcde3da25b02af33b6 100644
--- a/src/comm/ros_bridge/src/ROSBridge.cpp
+++ b/src/comm/ros_bridge/src/ROSBridge.cpp
@@ -3,8 +3,8 @@
 ROSBridge::ROSBridge::ROSBridge() :
     _casePacker(&_typeFactory, &_jsonFactory)
   , _rbc("localhost:9090")
-  , _topicPublisher(&_casePacker, &_rbc)
-  , _topicSubscriber(&_casePacker, &_rbc)
+  , _topicPublisher(_casePacker, _rbc)
+  , _topicSubscriber(_casePacker, _rbc)
 {
 
 }
@@ -36,3 +36,8 @@ void ROSBridge::ROSBridge::reset()
     _topicSubscriber.reset();
 }
 
+bool ROSBridge::ROSBridge::connected()
+{
+    return _rbc.connected();
+}
+