789

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 \

src/Wima/NemoProgress.h

-#pragma once
-#include <QVector>
-
-#include "ros_bridge/include/GenericMessages.h"
-
-namespace NemoMsgs = ROSBridge::GenericMessages::NemoMsgs;
-typedef NemoMsgs::GenericProgress<long, QVector> QNemoProgress;

src/Wima/QNemoProgress.cc

+#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;
+}

src/Wima/QNemoProgress.h

+#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();
+};
+
+

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().

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) {

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

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;

src/comm/ros_bridge/include/ComPrivateInclude.cpp

 #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));
+}

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

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

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()));

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

src/comm/ros_bridge/include/Receiver.cpp

-#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)
-{
-
-}

src/comm/ros_bridge/include/Receiver.h

-#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;
-};
-
-}
-}

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

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

src/comm/ros_bridge/include/TopicSubscriber.cpp

+#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;
+
+}

src/comm/ros_bridge/include/TopicSubscriber.h

+#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);
+
+}
+}

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

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