diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index 7edc9e5c1249e6e4d7dc7dc463e01bd37b6c3e71..e08b969369ec838e155ed17c3e6160f69110dba7 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -430,9 +430,9 @@ HEADERS += \
     src/Snake/snake_geometry.h \
     src/Snake/snake_global.h \
     src/Wima/GeoPoint3D.h \
-    src/Wima/NemoProgress.h \
     src/Wima/Polygon2D.h \
     src/Wima/PolygonArray.h \
+    src/Wima/QNemoProgress.h \
     src/Wima/QtROSJsonFactory.h \
     src/Wima/QtROSTypeFactory.h \
     src/Wima/SnakeTiles.h \
@@ -479,10 +479,10 @@ HEADERS += \
     src/comm/ros_bridge/include/JsonMethodes.h \
     src/comm/ros_bridge/include/MessageTag.h \
     src/comm/ros_bridge/include/MessageTraits.h \
-    src/comm/ros_bridge/include/Receiver.h \
     src/comm/ros_bridge/include/RosBridgeClient.h \
     src/comm/ros_bridge/include/ThreadSafeQueue.h \
     src/comm/ros_bridge/include/TopicPublisher.h \
+    src/comm/ros_bridge/include/TopicSubscriber.h \
     src/comm/ros_bridge/include/TypeFactory.h \
     src/comm/ros_bridge/src/PackageBuffer.h \
     src/comm/utilities.h
@@ -492,10 +492,11 @@ SOURCES += \
     src/Snake/snake_geometry.cpp \
     src/Wima/GeoPoint3D.cpp \
     src/Wima/PolygonArray.cc \
+    src/Wima/QNemoProgress.cc \
     src/comm/ros_bridge/include/ComPrivateInclude.cpp \
     src/comm/ros_bridge/include/MessageTag.cpp \
-    src/comm/ros_bridge/include/Receiver.cpp \
     src/comm/ros_bridge/include/TopicPublisher.cpp \
+    src/comm/ros_bridge/include/TopicSubscriber.cpp \
     src/comm/ros_bridge/src/CasePacker.cpp \
     src/Wima/WimaControllerDetail.cc \
     src/Wima/snaketile.cpp \
diff --git a/src/Wima/NemoProgress.h b/src/Wima/NemoProgress.h
deleted file mode 100644
index b8cc518f924eea3f047a90186624d4ba0ad73c04..0000000000000000000000000000000000000000
--- a/src/Wima/NemoProgress.h
+++ /dev/null
@@ -1,7 +0,0 @@
-#pragma once
-#include <QVector>
-
-#include "ros_bridge/include/GenericMessages.h"
-
-namespace NemoMsgs = ROSBridge::GenericMessages::NemoMsgs;
-typedef NemoMsgs::GenericProgress<long, QVector> QNemoProgress;
diff --git a/src/Wima/QNemoProgress.cc b/src/Wima/QNemoProgress.cc
new file mode 100644
index 0000000000000000000000000000000000000000..00754c7396cb980b9a718aaea3dc813ec2affafb
--- /dev/null
+++ b/src/Wima/QNemoProgress.cc
@@ -0,0 +1,28 @@
+#include "QNemoProgress.h"
+
+QNemoProgress::QNemoProgress(QObject *parent) :
+    ProgressBase()
+  , QObject(parent)
+{
+
+}
+
+QNemoProgress::QNemoProgress(const QNemoProgress &other, QObject *parent):
+    ProgressBase(other)
+  , QObject(parent)
+{
+
+}
+
+QNemoProgress *QNemoProgress::Clone() const {
+    return new QNemoProgress(*this, this->parent());
+}
+
+QVector<int> &QNemoProgress::progress() {
+    emit QNemoProgress::progressChanged();
+    return _progress;
+}
+
+const QVector<int> &QNemoProgress::progress() const {
+    return _progress;
+}
diff --git a/src/Wima/QNemoProgress.h b/src/Wima/QNemoProgress.h
new file mode 100644
index 0000000000000000000000000000000000000000..bc0d57852e21526810e8e82f515ad3890802748a
--- /dev/null
+++ b/src/Wima/QNemoProgress.h
@@ -0,0 +1,26 @@
+#pragma once
+#include <QVector>
+#include <QObject>
+
+#include "ros_bridge/include/GenericMessages.h"
+
+namespace NemoMsgs = ROSBridge::GenericMessages::NemoMsgs;
+typedef NemoMsgs::GenericProgress<int, QVector> ProgressBase;
+
+class QNemoProgress : public ProgressBase, public QObject {
+public:
+    QNemoProgress(QObject *parent = nullptr);
+    QNemoProgress(const QNemoProgress &other, QObject *parent = nullptr);
+
+
+
+    virtual QNemoProgress *Clone() const override;
+
+    virtual const QVector<int> &progress(void) const override;
+    virtual       QVector<int> &progress(void)       override;
+
+    signals:
+    void progressChanged();
+};
+
+
diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc
index 2e73f5501239afa1609b92070ae81a0d1578429b..3c5f693893c64cff319dc0cc40cc438ea9d7c2e9 100644
--- a/src/Wima/WimaController.cc
+++ b/src/Wima/WimaController.cc
@@ -4,10 +4,11 @@
 #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 "QtROSJsonFactory.h"
 #include "QtROSTypeFactory.h"
-#include "NemoProgress.h"
+#include "QNemoProgress.h"
 
 #include "time.h"
 #include "assert.h"
@@ -589,7 +590,11 @@ bool WimaController::_fetchContainerData()
         return false;
 
     {
-        // Get tiles.
+        // 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 ) {
@@ -619,103 +624,6 @@ bool WimaController::_fetchContainerData()
             }
             _snakeTilesLocal.polygons().append(Tile);
         }
-
-        QtROSJsonFactory JsonFactory;
-        QScopedPointer<rapidjson::Document> doc(JsonFactory.create(_snakeTilesLocal));
-
-        auto &temp = _scenario.getOrigin();
-        ::GeoPoint3D origin(temp[0], temp[1], temp[2]);
-        QScopedPointer<rapidjson::Document> doc2(JsonFactory.create(origin));
-
-        cout.precision(10);
-        cout << "Origin" << endl;
-        rapidjson::OStreamWrapper out(std::cout);
-        rapidjson::Writer<rapidjson::OStreamWrapper> writer(out);
-        doc2->Accept(writer);
-        std::cout << std::endl << std::endl;
-
-        cout << "Snake Tiles" << endl;
-        rapidjson::Writer<rapidjson::OStreamWrapper> writer2(out);
-        //doc->Accept(writer2);
-        std::cout << std::endl << std::endl;
-
-        QtROSTypeFactory TypeFactory;
-      ::GeoPoint3D origin_same;
-        TypeFactory.create(*doc2.data(), origin_same);
-
-        cout << "Origin2" << endl;
-        std::cout << origin_same << std::endl;
-
-        std::cout << "TypeFactory test true: ";
-        bool isSame = origin_same == origin;
-        std::cout << isSame << endl;
-
-
-        ::SnakeTilesLocal tiles_same;
-          TypeFactory.create(*doc.data(), tiles_same);
-
-
-          Polygon2D tile1;
-          tile1.path().push_back(QPointF(1,0));
-          tile1.path().push_back(QPointF(1,1));
-          tile1.path().push_back(QPointF(1,2));
-          Polygon2D tile2;
-          tile2.path().push_back(QPointF(2,0));
-          tile2.path().push_back(QPointF(2,1));
-          tile2.path().push_back(QPointF(2,2));
-          Polygon2D tile3;
-          tile3.path().push_back(QPointF(3,0));
-          tile3.path().push_back(QPointF(3,1));
-          tile3.path().push_back(QPointF(3,2));
-          SnakeTilesLocal tilesSmall;
-          tilesSmall.polygons().push_back(tile1);
-          tilesSmall.polygons().push_back(tile2);
-          tilesSmall.polygons().push_back(tile3);
-
-
-          QScopedPointer<rapidjson::Document> jsonTileSmall(JsonFactory.create(tilesSmall));
-
-          SnakeTilesLocal tilesSmallSame;
-          TypeFactory.create(*jsonTileSmall.data(), tilesSmallSame);
-
-          QScopedPointer<rapidjson::Document> jsonTileSmallSame(JsonFactory.create(tilesSmallSame));
-
-          cout << "Snake Tiles small" << endl;
-          rapidjson::Writer<rapidjson::OStreamWrapper> writer4(out);
-          jsonTileSmall->Accept(writer4);
-          std::cout << std::endl << std::endl;
-
-          cout << "Snake Tiles small same" << endl;
-          rapidjson::Writer<rapidjson::OStreamWrapper> writer5(out);
-          jsonTileSmallSame->Accept(writer5);
-          std::cout << std::endl << std::endl;
-
-          // progress
-          QNemoProgress progress;
-          progress.progress().push_back(1);
-          progress.progress().push_back(2);
-          progress.progress().push_back(3);
-          progress.progress().push_back(4);
-          progress.progress().push_back(13);
-          progress.progress().push_back(600);
-
-
-          QScopedPointer<rapidjson::Document> jsonProgress(JsonFactory.create(progress));
-          cout << "Progress" << endl;
-          rapidjson::Writer<rapidjson::OStreamWrapper> writer6(out);
-          jsonProgress->Accept(writer6);
-          std::cout << std::endl << std::endl;
-
-          QNemoProgress progressSame;
-          TypeFactory.create(*jsonProgress.data(), progressSame);
-          cout << "Progress Type created." << endl;
-
-          QScopedPointer<rapidjson::Document> jsonProgressSame(JsonFactory.create(progressSame));
-
-          cout << "Progress same" << endl;
-          rapidjson::Writer<rapidjson::OStreamWrapper> writer7(out);
-          jsonProgressSame->Accept(writer7);
-          std::cout << std::endl << std::endl;
     }
 
 
@@ -1097,19 +1005,8 @@ void WimaController::_eventTimerHandler()
     if ( snakeEventLoopTicker.ready() ) {
         if ( _enableSnake.rawValue().toBool() && _localPlanDataValid && !_snakeCalcInProgress && _scenarioDefinedBool) {
 
-            _snakeProgress.clear();
-            long n = _scenario.getTilesENU().size();
-            _snakeProgress.reserve(n);
-            std::srand(time(NULL));
-            for (long i=0; i<n; ++i){
-                int r{rand()%200};
-                if ( r > 100 )
-                    r = 100;
-                _snakeProgress.append(r);
-            }
-
             _snakeWorker.setScenario(_scenario);
-            _snakeWorker.setProgress(_snakeProgress);
+            _snakeWorker.setProgress(_snakeProgress.progress());
             _snakeWorker.setLineDistance(_snakeLineDistance.rawValue().toDouble());
             _snakeWorker.setMinTransectLength(_snakeMinTransectLength.rawValue().toDouble());
             _setSnakeCalcInProgress(true);
@@ -1122,110 +1019,15 @@ void WimaController::_eventTimerHandler()
     if (rosBridgeTicker.ready()) {
 //        using namespace ros_bridge;
 
-        _pRosBridge->publish(_snakeTilesLocal, "/snake/snake_tiles");
-//        // Time
-//        class Time time(1, 2);
-//        rapidjson::Document doc(rapidjson::kObjectType);
-//        // Write to stdout
-//        cout << "Time" << endl;
-//        cout << "Return value : " << time.toJson(doc, doc.GetAllocator()) << std::endl;
-//        rapidjson::OStreamWrapper out(std::cout);
-//        rapidjson::Writer<rapidjson::OStreamWrapper> writer(out);
-//        doc.Accept(writer);
-//        std::cout << std::endl << std::endl;
-
-
-//        // Header
-//        Header header(1, &time, "/map");
-//        // Write to stdout
-//        cout << "Header" << endl;
-//        rapidjson::Document doc2(rapidjson::kObjectType);
-//        cout << "Return value : " << header.toJson(doc2, doc2.GetAllocator()) << std::endl;
-//        rapidjson::Writer<rapidjson::OStreamWrapper> writer2(out);
-//        doc2.Accept(writer2);
-//        std::cout << std::endl << std::endl;
-
-
-//        // Point 3D
-//        QVector3D p1(1, 2, 3);
-//        typedef Point32<QVector3D> PointWrapper;
-//        PointWrapper pw1(&p1);
-//        // Write to stdout
-//        cout << "Point 3D" << endl;
-//        rapidjson::Document doc3(rapidjson::kObjectType);
-//        cout << "Return value : " << pw1.toJson(doc3, doc3.GetAllocator()) << std::endl;
-//        rapidjson::Writer<rapidjson::OStreamWrapper> writer3(out);
-//        doc3.Accept(writer3);
-//        std::cout << std::endl << std::endl;
-
-
-//        // Point 2D
-//        QPoint p2D(1, 2);
-//        p2D.x();
-//        Point32<QPoint> pw2D(&p2D);
-//        // Write to stdout
-//        cout << "Point 2D" << endl;
-//        rapidjson::Document docI(rapidjson::kObjectType);
-//        cout << "Return value : " << pw2D.toJson(docI, docI.GetAllocator()) << std::endl;
-//        rapidjson::Writer<rapidjson::OStreamWrapper> writerI(out);
-//        docI.Accept(writerI);
-//        std::cout << std::endl << std::endl;
-
-
-//        // Polygon
-//        QVector3D p2(4, 5, 6);
-//        QVector3D p3(7, 8, 9);
-//        PointWrapper pw2(&p2);
-//        PointWrapper pw3(&p3);
-//        QVector<PointWrapper> pointList;
-//        pointList.append(pw1);
-//        pointList.append(pw2);
-//        pointList.append(pw3);
-//        typedef Polygon<PointWrapper, QVector> Poly;
-//        Poly polyW(&pointList);
-//        // Write to stdout
-//        cout << "Polygon" << endl;
-//        rapidjson::Document doc4(rapidjson::kObjectType);
-//        cout << "Return value : " << polyW.toJson(doc4, doc4.GetAllocator()) << std::endl;
-//        rapidjson::Writer<rapidjson::OStreamWrapper> writer4(out);
-//        doc4.Accept(writer4);
-//        std::cout << std::endl << std::endl;
-
-
-//        // PolygonStamped
-//        typedef PolygonStamped<PointWrapper, Poly> PolyWrapper;
-//        PolyWrapper polyStamped(&header, &polyW);
-//        // Write to stdout
-//        cout << "PolygonStamped" << endl;
-//        rapidjson::Document doc5(rapidjson::kObjectType);
-//        cout << "Return value : " << polyStamped.toJson(doc5, doc5.GetAllocator()) << std::endl;
-//        rapidjson::Writer<rapidjson::OStreamWrapper> writer5(out);
-//        doc5.Accept(writer5);
-//        std::cout << std::endl << std::endl;
-
-
-//        // PolygonArray
-//        // Define second Polygon
-//        QVector3D p4(10, 11, 12);
-//        QVector3D p5(13, 14, 15);
-//        QVector3D p6(16, 17, 18);
-//        PointWrapper pw4(&p4);
-//        PointWrapper pw5(&p5);
-//        PointWrapper pw6(&p6);
-//        QVector<PointWrapper> pointList2{pw4, pw5, pw6};
-//        Poly polyW2(&pointList2);
-//        typedef PolygonArray<PointWrapper, Poly, QVector> PolyArray;
-//        QVector<Poly*> polygons{&polyW, &polyW2};
-//        PolyArray polyArray{};
-//        polyArray.setHeader(&header);
-//        polyArray.setPolygons(&polygons);
-//        // Write to stdout
-//        cout << "PolygonStamped" << endl;
-//        rapidjson::Document doc6(rapidjson::kObjectType);
-//        cout << "Return value : " << polyArray.toJson(doc6, doc6.GetAllocator()) << std::endl;
-//        rapidjson::Writer<rapidjson::OStreamWrapper> writer6(out);
-//        doc6.Accept(writer6);
-//        std::cout << std::endl << std::endl;
+        _pRosBridge->publish(_snakeTilesLocal, "/snake/tiles");
+        _pRosBridge->publish(_snakeOrigin, "/snake/origin");
+
+        using namespace std::placeholders;
+        auto callBack = std::bind(ProgressFromJson,
+                                  std::ref(*(_pRosBridge->casePacker())),
+                                  _1,
+                                  std::ref(_snakeProgress));
+        _pRosBridge->subscribe("/nemo/progress", callBack);
     }
 }
 
@@ -1596,5 +1398,7 @@ void WimaController::_saveCurrentMissionItemsToBuffer()
         _missionItemsBuffer.append(_currentMissionItems.removeAt(0));
 }
 
-
-
+void ProgressFromJson(const ROSBridge::CasePacker &casePacker, JsonDocUPtr pDoc, QNemoProgress &progress)
+{
+    casePacker.unpack(pDoc, progress);
+}
diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h
index 6db2aa0f41686a76556d05971bd7a275fad01543..a54ae208f103b3239032196820585651cd5c68b8 100644
--- a/src/Wima/WimaController.h
+++ b/src/Wima/WimaController.h
@@ -31,6 +31,7 @@
 #include "SnakeTiles.h"
 #include "SnakeTilesLocal.h"
 #include "GeoPoint3D.h"
+#include "QNemoProgress.h"
 
 #include "ros_bridge/include/ROSBridge.h"
 
@@ -43,12 +44,16 @@
 
 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:
 
     enum SnakeConnectionStatus {Connected = 1, NotConnected = 0};
@@ -95,7 +100,7 @@ public:
     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(QList<int>               snakeProgress               READ snakeProgress              NOTIFY snakeProgressChanged)
+    Q_PROPERTY(QVector<int>             *snakeProgress              READ snakeProgress              NOTIFY snakeProgressChanged)
 
 
 
@@ -120,7 +125,7 @@ public:
     Fact*                       showAllMissionItems    (void)           { return &_showAllMissionItems; }
     Fact*                       showCurrentMissionItems(void)           { return &_showCurrentMissionItems; }
     Fact*                       flightSpeed            (void)           { return &_flightSpeed; }
-    Fact*                       arrivalReturnSpeed     (void)           { return &_arrivalReturnSpeed; };
+    Fact*                       arrivalReturnSpeed     (void)           { return &_arrivalReturnSpeed; }
     Fact*                       altitude               (void)           { return &_altitude; }
     Fact*                       reverse                (void)           { return &_reverse; }
 
@@ -132,7 +137,7 @@ public:
     Fact*                       snakeMinTransectLength (void)           { return &_snakeMinTransectLength;}
     QmlObjectListModel*         snakeTiles             (void)           { return _snakeTiles.QmlObjectListModel();}
     QVariantList                snakeTileCenterPoints  (void)           { return _snakeTileCenterPoints;}
-    QList<int>                  snakeProgress          (void)           { return _snakeProgress;}
+    QVector<int>               *snakeProgress          (void)           { return &_snakeProgress.progress();}
 
     bool                        uploadOverrideRequired (void) const;
     double                      phaseDistance          (void) const;
@@ -330,15 +335,18 @@ private:
     SettingsFact                 _snakeMinTileArea;
     SettingsFact                 _snakeLineDistance;
     SettingsFact                 _snakeMinTransectLength;
+    ::GeoPoint3D                 _snakeOrigin;
     SnakeTiles                   _snakeTiles; // tiles
     SnakeTilesLocal              _snakeTilesLocal; // tiles local coordinate system
     QVariantList                 _snakeTileCenterPoints;
-    QList<int>                   _snakeProgress; // measurement progress
+    QNemoProgress                _snakeProgress; // measurement progress
 
-    QScopedPointer<ROSBridge::ROSBridge>    _pRosBridge;
+    ROSBridgePtr                 _pRosBridge;
 
 };
 
+void ProgressFromJson(const ROSBridge::CasePacker &casePacker, JsonDocUPtr pDoc, QNemoProgress &progress);
+
 /*
  * The following explains the structure of
  * _missionController.visualItems().
diff --git a/src/Wima/WimaControllerDetail.cc b/src/Wima/WimaControllerDetail.cc
index f1bdb46e1c16f83f49f21fb26290c399d894c76e..b0cd097f85b472a9e2d1311f30e25ed513775d33 100644
--- a/src/Wima/WimaControllerDetail.cc
+++ b/src/Wima/WimaControllerDetail.cc
@@ -14,7 +14,7 @@ void SnakeWorker::setScenario(const Scenario &scenario)
     _scenario = scenario;
 }
 
-void SnakeWorker::setProgress(const QList<int> &progress)
+void SnakeWorker::setProgress(const QVector<int> &progress)
 {
     _progress.clear();
     for (auto p : progress) {
diff --git a/src/Wima/WimaControllerDetail.h b/src/Wima/WimaControllerDetail.h
index 7cb55996af251d77747652a4b09c9fdd762e12ae..729efe4ea013084dd345ec05126a594b83253159 100644
--- a/src/Wima/WimaControllerDetail.h
+++ b/src/Wima/WimaControllerDetail.h
@@ -30,7 +30,7 @@ public:
     SnakeWorker(QObject *parent = nullptr);
 
     void setScenario            (const Scenario  &scenario);
-    void setProgress            (const QList<int> &progress);
+    void setProgress            (const QVector<int> &progress);
     void setLineDistance        (double lineDistance);
     void setMinTransectLength   (double minTransectLength);
 
diff --git a/src/comm/ros_bridge/include/CasePacker.h b/src/comm/ros_bridge/include/CasePacker.h
index 1f24fc10c95cb3ed3c539113b7e46a4093450db2..5ecfc42e33fa6f2bb56d822075cc681bd49d8b14 100644
--- a/src/comm/ros_bridge/include/CasePacker.h
+++ b/src/comm/ros_bridge/include/CasePacker.h
@@ -19,29 +19,29 @@ public:
     CasePacker(TypeFactory *typeFactory, JsonFactory *jsonFactory);
 
     template<class T>
-    JsonDoc *pack(const T &msg, const std::string &topic) const
+    JsonDocUPtr pack(const T &msg, const std::string &topic) const
     {
-        JsonDoc *docPt(_jsonFactory->create(msg));
+        JsonDocUPtr docPt(_jsonFactory->create(msg));
         std::string messageType = T::Group::messageType();
-        addTag(*docPt, topic, messageType.c_str());
+        addTag(docPt, topic, messageType.c_str());
         return docPt;
     }
 
     template<class T>
-    bool unpack(JsonDoc &doc, T &msg) const {
-        removeTag(doc);
-        return _typeFactory->create(doc, msg);
+    bool unpack(JsonDocUPtr &pDoc, T &msg) const {
+        removeTag(pDoc);
+        return _typeFactory->create(pDoc, msg);
     }
 
 
-    bool getTag(const JsonDoc &doc, Tag &tag) const;
-    void addTag        (JsonDoc &doc,
+    bool getTag(const JsonDocUPtr &pDoc, Tag &tag) const;
+    void addTag        (JsonDocUPtr &doc,
                          const std::string &topic,
                          const std::string &messageType) const;
-    void addTag        (JsonDoc &doc, const Tag &tag) const;
-    void removeTag     (JsonDoc &doc) const;
-    bool getTopic      (const JsonDoc &doc, std::string &topic) const;
-    bool getMessageType(const JsonDoc &doc, std::string &messageType) const;
+    void addTag        (JsonDocUPtr &doc, const Tag &tag) const;
+    void removeTag     (JsonDocUPtr &pDoc) const;
+    bool getTopic      (const JsonDocUPtr &pDoc, std::string &topic) const;
+    bool getMessageType(const JsonDocUPtr &pDoc, std::string &messageType) const;
 
     static const char* topicKey;
     static const char* messageTypeKey;
diff --git a/src/comm/ros_bridge/include/ComPrivateInclude.cpp b/src/comm/ros_bridge/include/ComPrivateInclude.cpp
index b926bf0e81de16d29f38ff77ffdd3b07200ba636..ac583a76d1f01f85b51d1ac5a28048656b428977 100644
--- a/src/comm/ros_bridge/include/ComPrivateInclude.cpp
+++ b/src/comm/ros_bridge/include/ComPrivateInclude.cpp
@@ -1 +1,13 @@
 #include "ros_bridge/include/ComPrivateInclude.h"
+#include <functional>
+
+std::size_t ROSBridge::ComPrivate::getHash(const std::string &str)
+{
+    std::hash<std::string> hash;
+    return hash(str);
+}
+
+std::size_t ROSBridge::ComPrivate::getHash(const char *str)
+{
+    return ROSBridge::ComPrivate::getHash(std::string(str));
+}
diff --git a/src/comm/ros_bridge/include/ComPrivateInclude.h b/src/comm/ros_bridge/include/ComPrivateInclude.h
index d9e3362a16e4618f32734666eeee000c3cec3c7f..c83cdb97406f4214051835cada29afd392ff07d1 100644
--- a/src/comm/ros_bridge/include/ComPrivateInclude.h
+++ b/src/comm/ros_bridge/include/ComPrivateInclude.h
@@ -5,6 +5,7 @@
 
 #include <memory>
 #include <deque>
+#include <set>
 
 namespace ROSBridge {
 namespace ComPrivate {
@@ -13,11 +14,16 @@ typedef MessageTag Tag;
 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;
 
 static const char* _topicAdvertiserKey = "topic_advertiser";
 static const char* _topicPublisherKey =  "topic_publisher";
 //static const char* _topicAdvertiserKey = "service_advertiser";
 static const char* _topicSubscriberKey = "topic_subscriber";
 
+std::size_t getHash(const std::string &str);
+std::size_t getHash(const char *str);
+
 }
 }
diff --git a/src/comm/ros_bridge/include/GenericMessages.h b/src/comm/ros_bridge/include/GenericMessages.h
index 0cf74328d9873bd60ff899b35d447d08a3bbab44..a27ed74fb05f0b69acb8514a3fdb07c6173663a9 100644
--- a/src/comm/ros_bridge/include/GenericMessages.h
+++ b/src/comm/ros_bridge/include/GenericMessages.h
@@ -332,11 +332,11 @@ public:
     GenericProgress(const ContainterType<IntType> &progress) :_progress(progress){}
     GenericProgress(const GenericProgress &p) :_progress(p.progress()){}
 
-    GenericProgress *Clone() const override { return new GenericProgress(*this); }
+    virtual GenericProgress *Clone() const override { return new GenericProgress(*this); }
 
-    const ContainterType<IntType> &progress(void) const {return _progress;}
-          ContainterType<IntType> &progress(void)       {return _progress;}
-private:
+    virtual const ContainterType<IntType> &progress(void) const {return _progress;}
+    virtual       ContainterType<IntType> &progress(void)       {return _progress;}
+protected:
     ContainterType<IntType> _progress;
 };
 
diff --git a/src/comm/ros_bridge/include/JsonMethodes.h b/src/comm/ros_bridge/include/JsonMethodes.h
index 8d62f10322137602499b124fcae116fa3ee4453f..65d5e556483a03fc8e354d5b73049e8a9681b58f 100644
--- a/src/comm/ros_bridge/include/JsonMethodes.h
+++ b/src/comm/ros_bridge/include/JsonMethodes.h
@@ -218,6 +218,7 @@ bool fromJson(const rapidjson::Value &value, PolygonType &poly)
         return false;
     }
     const auto &jsonArray = value["points"].GetArray();
+    poly.points().clear();
     poly.points().reserve(jsonArray.Size());
     typedef decltype (poly.points()[0]) PointTypeCVR;
     typedef typename boost::remove_cv<typename boost::remove_reference<PointTypeCVR>::type>::type PointType;
@@ -420,14 +421,21 @@ using namespace ROSBridge::JsonMethodes::GeometryMsgs;
 
         //! \note \p p has member \fn likelihood().
         template <class PolygonArrayType, int  k>
-        void likelihoodToJson(const PolygonArrayType &p, rapidjson::Value &likelyhood, rapidjson::Document::AllocatorType &allocator, Int2Type<k>){
+        void likelihoodToJson(const PolygonArrayType &p,
+                              rapidjson::Value &likelyhood,
+                              rapidjson::Document::AllocatorType &allocator,
+                              Int2Type<k>){
+            p.likelyhood().clear();
             for(unsigned long i=0; i < (unsigned long)p.likelyhood().size(); ++i)
                 likelyhood.PushBack(rapidjson::Value().SetFloat(p.likelyhood()[i]), allocator);
         }
 
         //! \note \p p has no member \fn likelihood().
         template <class PolygonArrayType>
-        void likelihoodToJson(const PolygonArrayType &p, rapidjson::Value &likelyhood, rapidjson::Document::AllocatorType &allocator, Int2Type<0>){
+        void likelihoodToJson(const PolygonArrayType &p,
+                              rapidjson::Value &likelyhood,
+                              rapidjson::Document::AllocatorType &allocator,
+                              Int2Type<0>){
             for(unsigned long i=0; i < (unsigned long)p.polygons().size(); ++i)
                 likelyhood.PushBack(rapidjson::Value().SetFloat(0), allocator); // use zero!
         }
@@ -435,6 +443,7 @@ using namespace ROSBridge::JsonMethodes::GeometryMsgs;
         //! \note \p p has member \fn labels().
         template <class PolygonArrayType, int k>
         void setLabels(const rapidjson::Value &doc, PolygonArrayType &p, Int2Type<k>){
+            p.labels().clear();
             for(unsigned long i=0; i < (unsigned long)doc.Size(); ++i)
                 p.labels().push_back(doc[i]);
         }
@@ -587,6 +596,7 @@ using namespace ROSBridge::JsonMethodes::GeometryMsgs;
 
 
         const auto &polyStampedJson = value["polygons"];
+        p.polygons().clear();
         p.polygons().reserve(polyStampedJson.Size());
         typedef decltype (p.polygons()[0]) PolyStampedCVR;
         typedef typename boost::remove_cv<typename boost::remove_reference<PolyStampedCVR>::type>::type
@@ -649,6 +659,7 @@ namespace Progress {
 
         const auto& jsonProgress = value["progress"];
         unsigned long sz = jsonProgress.Size();
+        p.progress().clear();
         p.progress().reserve(sz);
         for (unsigned long i=0; i < sz; ++i)
             p.progress().push_back(std::int8_t(jsonProgress[i].GetInt()));
diff --git a/src/comm/ros_bridge/include/ROSBridge.h b/src/comm/ros_bridge/include/ROSBridge.h
index 131908231d8a319330a25c3eb2624df916986414..3c0f9eed25ce55752fd5dbcc76b99e3dd90e01d0 100644
--- a/src/comm/ros_bridge/include/ROSBridge.h
+++ b/src/comm/ros_bridge/include/ROSBridge.h
@@ -5,7 +5,7 @@
 #include "ros_bridge/include/TypeFactory.h"
 #include "ros_bridge/include/JsonFactory.h"
 #include "ros_bridge/include/TopicPublisher.h"
-#include "ros_bridge/include/Receiver.h"
+#include "ros_bridge/include/TopicSubscriber.h"
 
 #include <memory>
 #include <tuple>
@@ -26,23 +26,22 @@ public:
     void publish(T &msg, const std::string &topic){
         _topicPublisher.publish(msg, topic);
     }
+    void publish(JsonDocUPtr doc);
+
+    void subscribe(const char *topic, const std::function<(JsonDocUPtr)> &callBack);
+
+    const CasePacker *casePacker() const;
 
     void start();
     void stop();
-    bool messagesAvailable();
-    bool showMessageTag(Tag &tag);
-    template<class T>
-    void receive(T &msg){
-        _receiver.receive(msg);
-    }
 
 private:
     TypeFactory             _typeFactory;
     JsonFactory             _jsonFactory;
     CasePacker              _casePacker;
     RosbridgeWsClient       _rbc;
-    ComPrivate::TopicPublisher _topicPublisher;
-    ComPrivate::Receiver    _receiver;
+    ComPrivate::TopicPublisher     _topicPublisher;
+    ComPrivate::TopicSubscriber    _topicSubscriber;
 
 };
 }
diff --git a/src/comm/ros_bridge/include/Receiver.cpp b/src/comm/ros_bridge/include/Receiver.cpp
deleted file mode 100644
index a329f8d45ba52ac908670465413db181ae6e76ba..0000000000000000000000000000000000000000
--- a/src/comm/ros_bridge/include/Receiver.cpp
+++ /dev/null
@@ -1,17 +0,0 @@
-#include "Receiver.h"
-
-
-ROSBridge::ComPrivate::Receiver::Receiver(ROSBridge::CasePacker *casePacker, RosbridgeWsClient *rbc, ROSBridge::TypeFactory *typeFactory)
-{
-
-}
-
-bool ROSBridge::ComPrivate::Receiver::messagesAvailable()
-{
-
-}
-
-bool ROSBridge::ComPrivate::Receiver::showMessageTag(ROSBridge::ComPrivate::Tag &tag)
-{
-
-}
diff --git a/src/comm/ros_bridge/include/Receiver.h b/src/comm/ros_bridge/include/Receiver.h
deleted file mode 100644
index 20bb74f488b289e175ed336755efe198b088e63f..0000000000000000000000000000000000000000
--- a/src/comm/ros_bridge/include/Receiver.h
+++ /dev/null
@@ -1,37 +0,0 @@
-#pragma once
-
-#include "ros_bridge/include/ComPrivateInclude.h"
-#include "ros_bridge/include/RosBridgeClient.h"
-#include "ros_bridge/include/TypeFactory.h"
-#include "ros_bridge/include/CasePacker.h"
-
-namespace ROSBridge {
-namespace ComPrivate {
-
-class Receiver
-{
-public:
-    Receiver() = delete;
-    Receiver(CasePacker *casePacker, RosbridgeWsClient *rbc, TypeFactory *typeFactory);
-
-
-    template<class T>
-    void receive(T &){
-    }
-    bool messagesAvailable();
-    bool showMessageTag(Tag &tag);
-
-private:
-    static const char *_topicAdvertiserKey;
-
-    JsonQueue           _queue;
-    std::mutex          _queueMutex;
-    std::atomic<bool>   _stopFlag;
-    JsonFactory        *_jsonFactory;
-    CasePacker         *_casePacker;
-    //ThreadPtr           _pThread;
-    RosbridgeWsClient  *_rbc;
-};
-
-}
-}
diff --git a/src/comm/ros_bridge/include/TopicPublisher.cpp b/src/comm/ros_bridge/include/TopicPublisher.cpp
index cacfe5adf5e0df81660e592000172e4d05e86775..00260b2181ffd55d1e2f2f9ad57264d4b0e7dccd 100644
--- a/src/comm/ros_bridge/include/TopicPublisher.cpp
+++ b/src/comm/ros_bridge/include/TopicPublisher.cpp
@@ -5,6 +5,7 @@ void ROSBridge::ComPrivate::transmittLoop(const ROSBridge::CasePacker &casePacke
                                           RosbridgeWsClient &rbc,
                                           ROSBridge::ComPrivate::JsonQueue &queue,
                                           std::mutex &queueMutex,
+                                          HashSet &advertisedTopicsHashList,
                                           const std::atomic<bool> &stopFlag)
 {
     rbc.addClient(ROSBridge::ComPrivate::_topicPublisherKey);
@@ -31,14 +32,27 @@ void ROSBridge::ComPrivate::transmittLoop(const ROSBridge::CasePacker &casePacke
 
         // Get tag from Json message and remove it.
         Tag tag;
-        bool ret = casePacker.getTag(*pJsonDoc.get(), tag);
+        bool ret = casePacker.getTag(pJsonDoc, tag);
         assert(ret); // Json message does not contain a tag;
         (void)ret;
-        casePacker.removeTag(*pJsonDoc.get());
+        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() );
+        }
+        // Debug output.
+        //std::cout << "Hash Set size: " << advertisedTopicsHashList.size() << std::endl;
 
         // Send Json message.
         rbc.publish(tag.topic(), *pJsonDoc.get());
-    }
+    } // while loop
 }
 
 ROSBridge::ComPrivate::TopicPublisher::TopicPublisher(CasePacker *casePacker,
@@ -59,12 +73,14 @@ void ROSBridge::ComPrivate::TopicPublisher::start()
 {
     if ( !_stopFlag.load() )  // start called while thread running.
         return;
-    _stopFlag.store(false);
+    _stopFlag.store(false);    
+    _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(_stopFlag)));
 }
 
@@ -76,5 +92,6 @@ void ROSBridge::ComPrivate::TopicPublisher::stop()
     if ( !_pThread )
         return;
     _pThread->join();
-    _pThread.reset();
+    _pThread.reset();    
+    _rbc->removeClient(ROSBridge::ComPrivate::_topicAdvertiserKey);
 }
diff --git a/src/comm/ros_bridge/include/TopicPublisher.h b/src/comm/ros_bridge/include/TopicPublisher.h
index 7ffd067f1fc307324e1e4782e41866d511f76227..0e3f3e1024f44760e9710f0bd95f4f1ea8cb815b 100644
--- a/src/comm/ros_bridge/include/TopicPublisher.h
+++ b/src/comm/ros_bridge/include/TopicPublisher.h
@@ -9,6 +9,7 @@
 #include <deque>
 #include <atomic>
 #include <mutex>
+#include <set>
 
 namespace ROSBridge {
 namespace ComPrivate {
@@ -37,14 +38,14 @@ public:
     }
 
 private:
-    static const char *_topicAdvertiserKey;
-
     JsonQueue           _queue;
     std::mutex          _queueMutex;
     std::atomic<bool>   _stopFlag;
     CasePacker         *_casePacker;
     ThreadPtr           _pThread;
     RosbridgeWsClient  *_rbc;
+    HashSet            _advertisedTopicsHashList; // Not thread save! This container
+                                                  // is manipulated by transmittLoop only!
 };
 
 
@@ -52,6 +53,7 @@ void transmittLoop(const ROSBridge::CasePacker &casePacker,
                    RosbridgeWsClient &rbc,
                    ROSBridge::ComPrivate::JsonQueue &queue,
                    std::mutex &queueMutex,
+                   HashSet &advertisedTopicsHashList,
                    const std::atomic<bool> &stopFlag);
 
 
diff --git a/src/comm/ros_bridge/include/TopicSubscriber.cpp b/src/comm/ros_bridge/include/TopicSubscriber.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c1e8f0b46f42f77ad885ebf9688126e76aa24ecd
--- /dev/null
+++ b/src/comm/ros_bridge/include/TopicSubscriber.cpp
@@ -0,0 +1,80 @@
+#include "TopicSubscriber.h"
+
+
+ROSBridge::ComPrivate::TopicSubscriber::TopicSubscriber(
+        ROSBridge::CasePacker *casePacker,
+        RosbridgeWsClient *rbc) :
+    _casePacker(casePacker)
+  , _rbc(rbc)
+{
+
+}
+
+void ROSBridge::ComPrivate::TopicSubscriber::start()
+{
+    _rbc->addClient(ROSBridge::ComPrivate::_topicSubscriberKey);
+}
+
+void ROSBridge::ComPrivate::TopicSubscriber::stop()
+{
+    _rbc->removeClient(ROSBridge::ComPrivate::_topicSubscriberKey);
+}
+
+bool ROSBridge::ComPrivate::TopicSubscriber::subscribe(
+        const char *topic,
+        const std::function<void(JsonDocUPtr)> &callback)
+{
+    using namespace std::placeholders;
+    HashType hash = getHash(topic);
+    auto ret = _callbackMap.insert(std::make_pair(hash, callback)); //
+    if ( !ret.second )
+        return false; // Topic subscription already present.
+
+    auto f = std::bind(&ROSBridge::ComPrivate::subscriberCallback, hash, _callbackMap, _1, _2);
+    _rbc->subscribe(ROSBridge::ComPrivate::_topicSubscriberKey, topic, f);
+
+    return true;
+}
+
+
+using WsClient = SimpleWeb::SocketClient<SimpleWeb::WS>;
+void ROSBridge::ComPrivate::subscriberCallback(
+        const HashType &hash,
+        const ROSBridge::ComPrivate::TopicSubscriber::CallbackMap &map,
+        std::shared_ptr<WsClient::Connection>,
+        std::shared_ptr<WsClient::InMessage> in_message)
+{
+    // Parse document.
+    JsonDoc docFull;
+    docFull.Parse(in_message->string().c_str());
+    if ( docFull.HasParseError() ) {
+        std::cout << "Json document has parse error: "
+                  << in_message->string()
+                  << std::endl;
+        return;
+    } else if (!docFull.HasMember("msg")) {
+        std::cout << "Json document does not contain a message (\"msg\"): "
+                  << in_message->string()
+                  << std::endl;
+        return;
+    }
+
+
+//    std::cout << "Json document: "
+//              << in_message->string()
+//              << std::endl;
+
+    // Search callback.
+    auto it = map.find(hash);
+    if (it == map.end()) {
+        assert(false); // callback not found
+        return;
+    }
+
+    // Extract message and call callback.
+    JsonDocUPtr pDoc(new JsonDoc());
+    pDoc->CopyFrom(docFull["msg"].Move(), docFull.GetAllocator());
+    it->second(std::move(pDoc)); // Call callback.
+    return;
+
+}
diff --git a/src/comm/ros_bridge/include/TopicSubscriber.h b/src/comm/ros_bridge/include/TopicSubscriber.h
new file mode 100644
index 0000000000000000000000000000000000000000..ff65d2e814ef861ef5cea2a5e676863a6b1311a7
--- /dev/null
+++ b/src/comm/ros_bridge/include/TopicSubscriber.h
@@ -0,0 +1,40 @@
+#pragma once
+
+#include "ros_bridge/include/ComPrivateInclude.h"
+#include "ros_bridge/include/RosBridgeClient.h"
+#include "ros_bridge/include/TypeFactory.h"
+#include "ros_bridge/include/CasePacker.h"
+
+namespace ROSBridge {
+namespace ComPrivate {
+
+class TopicSubscriber
+{
+public:    
+    typedef std::function<void(JsonDocUPtr)> CallbackType;
+    typedef std::map<HashType, CallbackType> CallbackMap;
+
+    TopicSubscriber() = delete;
+    TopicSubscriber(CasePacker *casePacker, RosbridgeWsClient *rbc);
+
+    void start();
+    void stop();
+
+    //! @return Returns false if a subscription to this topic allready exists.
+    //!
+    //! @note Only one callback can be registered.
+    bool subscribe(const char* topic, const CallbackType &callback);
+
+private:
+    CasePacker         *_casePacker;
+    RosbridgeWsClient  *_rbc;
+    CallbackMap         _callbackMap;
+};
+
+void subscriberCallback(const HashType &hash,
+                        const TopicSubscriber::CallbackMap &map,
+                        std::shared_ptr<WsClient::Connection> /*connection*/,
+                        std::shared_ptr<WsClient::InMessage> in_message);
+
+}
+}
diff --git a/src/comm/ros_bridge/src/CasePacker.cpp b/src/comm/ros_bridge/src/CasePacker.cpp
index 4941aafe133561d45a044fba26b53c6113cc55c7..cd17974f34e579eb8d0c3c4cec6ebe64c6ecced7 100644
--- a/src/comm/ros_bridge/src/CasePacker.cpp
+++ b/src/comm/ros_bridge/src/CasePacker.cpp
@@ -10,60 +10,60 @@ ROSBridge::CasePacker::CasePacker(TypeFactory *typeFactory, JsonFactory *jsonFac
 
 }
 
-bool ROSBridge::CasePacker::getTag(const ROSBridge::CasePacker::JsonDoc &doc, Tag &tag) const
+bool ROSBridge::CasePacker::getTag(const JsonDocUPtr &pDoc, Tag &tag) const
 {
-    if( !getTopic(doc, tag.topic()) )
+    if( !getTopic(pDoc, tag.topic()) )
         return false;
-    if( !getMessageType(doc, tag.messageType()) )
+    if( !getMessageType(pDoc, tag.messageType()) )
         return false;
     return true;
 }
 
-void ROSBridge::CasePacker::addTag(JsonDoc &doc, const std::string &topic, const std::string &messageType) const
+void ROSBridge::CasePacker::addTag(JsonDocUPtr &pDoc, const std::string &topic, const std::string &messageType) const
 {
     using namespace ROSBridge;
     using namespace rapidjson;
 
     {
     // add topic
-    rapidjson::Value key(CasePacker::topicKey, doc.GetAllocator());
-    rapidjson::Value value(topic.c_str(), doc.GetAllocator());
-    doc.AddMember(key, value, doc.GetAllocator());
+    rapidjson::Value key(CasePacker::topicKey, pDoc->GetAllocator());
+    rapidjson::Value value(topic.c_str(), pDoc->GetAllocator());
+    pDoc->AddMember(key, value, pDoc->GetAllocator());
     }
 
     // add messageType
-    rapidjson::Value key(CasePacker::messageTypeKey, doc.GetAllocator());
-    rapidjson::Value value(messageType.c_str(), doc.GetAllocator());
-    doc.AddMember(key, value, doc.GetAllocator());
+    rapidjson::Value key(CasePacker::messageTypeKey, pDoc->GetAllocator());
+    rapidjson::Value value(messageType.c_str(), pDoc->GetAllocator());
+    pDoc->AddMember(key, value, pDoc->GetAllocator());
 }
 
-void ROSBridge::CasePacker::addTag(ROSBridge::CasePacker::JsonDoc &doc, const ROSBridge::CasePacker::Tag &tag) const
+void ROSBridge::CasePacker::addTag(JsonDocUPtr &doc, const ROSBridge::CasePacker::Tag &tag) const
 {
     addTag(doc, tag.topic(), tag.messageType());
 }
 
-void ROSBridge::CasePacker::removeTag(JsonDoc &doc) const
+void ROSBridge::CasePacker::removeTag(JsonDocUPtr &pDoc) const
 {
     using namespace ROSBridge;
     using namespace rapidjson;
-    if ( doc.HasMember(CasePacker::topicKey) )
-        doc.RemoveMember(CasePacker::topicKey);
-    if ( doc.HasMember(CasePacker::messageTypeKey) )
-        doc.RemoveMember(CasePacker::messageTypeKey);
+    if ( pDoc->HasMember(CasePacker::topicKey) )
+        pDoc->RemoveMember(CasePacker::topicKey);
+    if ( pDoc->HasMember(CasePacker::messageTypeKey) )
+        pDoc->RemoveMember(CasePacker::messageTypeKey);
 }
 
-bool ROSBridge::CasePacker::getTopic(const ROSBridge::CasePacker::JsonDoc &doc, std::string &topic) const
+bool ROSBridge::CasePacker::getTopic(const JsonDocUPtr &pDoc, std::string &topic) const
 {
-    if (!doc.HasMember(CasePacker::topicKey) || !doc[CasePacker::topicKey].IsString())
+    if (!pDoc->HasMember(CasePacker::topicKey) || !(*pDoc)[CasePacker::topicKey].IsString())
         return false;
-    topic = doc[CasePacker::topicKey].GetString();
+    topic = (*pDoc)[CasePacker::topicKey].GetString();
     return true;
 }
 
-bool ROSBridge::CasePacker::getMessageType(const ROSBridge::CasePacker::JsonDoc &doc, std::string &messageType) const
+bool ROSBridge::CasePacker::getMessageType(const JsonDocUPtr&pDoc, std::string &messageType) const
 {
-    if (!doc.HasMember(CasePacker::messageTypeKey) || !doc[CasePacker::messageTypeKey].IsString())
+    if (!pDoc->HasMember(CasePacker::messageTypeKey) || !(*pDoc)[CasePacker::messageTypeKey].IsString())
         return false;
-    messageType = doc[CasePacker::messageTypeKey].GetString();
+    messageType = (*pDoc)[CasePacker::messageTypeKey].GetString();
     return true;
 }
diff --git a/src/comm/ros_bridge/src/ROSBridge.cpp b/src/comm/ros_bridge/src/ROSBridge.cpp
index c9fdf9ed2e90a0fdeb362dd86b8c092477a66410..01cad18d557000af6a6d4ae450f9e4f666fb6ee4 100644
--- a/src/comm/ros_bridge/src/ROSBridge.cpp
+++ b/src/comm/ros_bridge/src/ROSBridge.cpp
@@ -4,28 +4,35 @@ ROSBridge::ROSBridge::ROSBridge() :
     _casePacker(&_typeFactory, &_jsonFactory)
   , _rbc("localhost:9090")
   , _topicPublisher(&_casePacker, &_rbc)
-  , _receiver(&_casePacker, &_rbc, &_typeFactory)
+  , _topicSubscriber(&_casePacker, &_rbc)
 {
 
 }
 
-void ROSBridge::ROSBridge::start()
+void ROSBridge::ROSBridge::publish(ROSBridge::ROSBridge::JsonDocUPtr doc)
 {
-    _topicPublisher.start();
+    _topicPublisher.publish(std::move(doc));
 }
 
-void ROSBridge::ROSBridge::stop()
+void ROSBridge::ROSBridge::subscribe(const char *topic, const std::function<void(JsonDocUPtr)> &callBack)
 {
-    _topicPublisher.stop();
+    _topicSubscriber.subscribe(topic, callBack);
 }
 
-bool ROSBridge::ROSBridge::messagesAvailable()
+const ROSBridge::CasePacker *ROSBridge::ROSBridge::casePacker() const
 {
-    return _receiver.messagesAvailable();
+    return &_casePacker;
 }
 
-bool ROSBridge::ROSBridge::showMessageTag(ROSBridge::ROSBridge::Tag &tag)
+void ROSBridge::ROSBridge::start()
 {
-    return _receiver.showMessageTag(tag);
+    _topicPublisher.start();
+    _topicSubscriber.start();
+}
+
+void ROSBridge::ROSBridge::stop()
+{
+    _topicPublisher.stop();
+    _topicSubscriber.stop();
 }