working on ROSBridge::Bride, code not compilable.

qgroundcontrol.pro

@@ -430,6 +430,7 @@ 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/QtROSJsonFactory.h \
@@ -472,9 +473,13 @@ HEADERS += \
     src/Wima/testplanimetrycalculus.h \
     src/Settings/WimaSettings.h \
     src/QmlControls/QmlObjectVectorModel.h \
+    src/comm/ros_bridge/include/GenericMessages.h \
     src/comm/ros_bridge/include/JsonMethodes.h \
     src/comm/ros_bridge/include/MessageTraits.h \
+    src/comm/ros_bridge/include/PackageBuffer.h \
     src/comm/ros_bridge/include/TypeFactory.h \
+    src/comm/ros_bridge/src/CasePacker.h \
+    src/comm/ros_bridge/src/PackageBuffer.h \
     src/comm/utilities.h
 SOURCES += \
     src/Snake/clipper/clipper.cpp \
@@ -482,6 +487,7 @@ SOURCES += \
     src/Snake/snake_geometry.cpp \
     src/Wima/GeoPoint3D.cpp \
     src/Wima/PolygonArray.cc \
+    src/comm/ros_bridge/src/CasePacker.cpp \
     src/comm/ros_bridge/src/ROSCommunicator.cpp \
     src/Wima/WimaControllerDetail.cc \
     src/Wima/snaketile.cpp \

src/Wima/NemoProgress.h

+#pragma once
+#include <QVector>
+
+#include "ros_bridge/include/JsonMethodes.h"
+
+namespace ProgressNS = ROSBridge::JsonMethodes::Progress;
+typedef ProgressNS::GenericProgress<long, QVector> NemoProgress;

src/Wima/Polygon2D.h

@@ -5,27 +5,39 @@
 
 #include "ros_bridge/include/MessageGroups.h"
 #include "ros_bridge/include/MessageBaseClass.h"
+#include "ros_bridge/include/JsonMethodes.h"
+
+namespace MsgGroupsNS = ROSBridge::MessageGroups;
+namespace PolyStampedNS = ROSBridge::JsonMethodes::PolygonStamped;
 
 typedef ROSBridge::MessageBaseClass<QString> ROSMsg;
-namespace MsgGroups = ROSBridge::MessageGroups;
 
-class Polygon2D : public QPolygonF, public ROSMsg{
+template <class PointType = QPointF, template <class, class...> class ContainerType = QVector>
+class Polygon2DTemplate : public ROSMsg{ //
+    typedef ROSBridge::JsonMethodes::Polygon::Polygon<PointType, ContainerType> Poly;
 public:
-    typedef MsgGroups::PolygonGroup Group;
+    typedef MsgGroupsNS::PolygonStampedGroup Group; // has no header
 
-    Polygon2D(){}
-    Polygon2D(const Polygon2D &other) : QPolygonF(other) , ROSMsg(){}
+    Polygon2DTemplate(){}
+    Polygon2DTemplate(const Polygon2DTemplate &other) : ROSMsg(), _polygon(other._polygon){}
 
-    Polygon2D& operator=(const Polygon2D& other) {
-        QPolygonF::operator=(other);
+    Polygon2DTemplate& operator=(const Polygon2DTemplate& other) {
+        this->_polygon = other._polygon;
         return *this;
     }
 
-    virtual Polygon2D*Clone() const { // ROSMsg
-        return new Polygon2D(*this);
-    }
+    const Poly &polygon() const {return _polygon;}
+          Poly &polygon()       {return _polygon;}
 
-    const Polygon2D &points() const {return *this;} // ROSMsg
-    Polygon2D &points() {return *this;} // ROSMsg
+  const ContainerType<PointType> &path() const {return _polygon.points();}
+        ContainerType<PointType> &path()       {return _polygon.points();}
+
+    virtual Polygon2DTemplate*Clone() const { // ROSMsg
+        return new Polygon2DTemplate(*this);
+    }
+private:
+    Poly _polygon;
 
 };
+
+typedef Polygon2DTemplate<> Polygon2D;

src/Wima/WimaController.cc

@@ -7,6 +7,7 @@
 
 #include "QtROSJsonFactory.h"
 #include "QtROSTypeFactory.h"
+#include "NemoProgress.h"
 
 #include "time.h"
 #include "assert.h"
@@ -609,7 +610,7 @@ bool WimaController::_fetchContainerData()
             Polygon2D Tile;
             for ( const auto &vertex : tile.outer()) {
                 QPointF QVertex(vertex.get<0>(), vertex.get<1>());
-                Tile.append(QVertex);
+                Tile.path().append(QVertex);
             }
             _snakeTilesLocal.polygons().append(Tile);
         }
@@ -649,7 +650,67 @@ bool WimaController::_fetchContainerData()
           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
+          NemoProgress 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));
+
+          NemoProgress progressSame;
+            TypeFactory.create(*jsonProgress.data(), progressSame);
+
+          QScopedPointer<rapidjson::Document> jsonProgressSame(JsonFactory.create(progressSame));
+
+          cout << "Snake Tiles small" << endl;
+          rapidjson::Writer<rapidjson::OStreamWrapper> writer6(out);
+          jsonProgress->Accept(writer6);
+          std::cout << std::endl << std::endl;
 
+          cout << "Snake Tiles small same" << endl;
+          rapidjson::Writer<rapidjson::OStreamWrapper> writer7(out);
+          jsonProgressSame->Accept(writer7);
+          std::cout << std::endl << std::endl;
     }
 
 

src/Wima/WimaController.h

@@ -26,8 +26,6 @@
 
 #include "snake.h"
 #include "WimaControllerDetail.h"
-//#include "snaketile.h"
-//#include "Polygon2D.h"
 #include "SnakeTiles.h"
 #include "SnakeTilesLocal.h"
 #include "GeoPoint3D.h"

src/comm/ros_bridge/include/JsonFactory.h

@@ -4,9 +4,10 @@
 #include "ros_bridge/include/JsonMethodes.h"
 #include "MessageBaseClass.h"
 #include "utilities.h"
-#include "ros_bridge/include/MessageTraits.h"
 
+#include "ros_bridge/include/MessageTraits.h"
 #include "ros_bridge/include/MessageGroups.h"
+#include "ros_bridge/include/GenericMessages.h"
 
 #include "boost/type_traits.hpp"
 #include "boost/type_traits/is_base_of.hpp"
@@ -21,13 +22,13 @@ class StdHeaderPolicy;
 //! The JsonFactory class is used to create \class rapidjson::Document documents containing ROS messages
 //! from classes derived from \class MessageBaseClass. Each class has a group mark (typedef ... Group) which allows the
 //! JsonFactory to determine the ROS message type it will create.
-template <class StringType = std::string, class HeaderPolicy = StdHeaderPolicy>
+template <class HeaderPolicy = StdHeaderPolicy>
 class JsonFactory : public HeaderPolicy
 {
-    typedef MessageBaseClass<StringType> ROSMsg;
+    typedef MessageBaseClass ROSMsg;
 public:
 
-    JsonFactory(){}
+    JsonFactory() : HeaderPolicy() {}
 
     //!
     //! \brief Creates a \class rapidjson::Document document containing a ROS mesage from \p msg.
@@ -64,7 +65,7 @@ private:
     rapidjson::Document *_create(const U &msg, Type2Type<MessageGroups::Point32Group>){
         using namespace ROSBridge;
         rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
-        bool ret = JsonMethodes::Point32::toJson<_Float32>(msg, *doc, doc->GetAllocator());
+        bool ret = JsonMethodes::GeometryMsgs::Point32::toJson<_Float32>(msg, *doc, doc->GetAllocator());
         assert(ret);
         (void)ret;
 
@@ -78,7 +79,7 @@ private:
     rapidjson::Document *_create(const U &msg, Type2Type<MessageGroups::GeoPointGroup>){
         using namespace ROSBridge;
         rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
-        bool ret = JsonMethodes::GeoPoint::toJson(msg, *doc, doc->GetAllocator());
+        bool ret = JsonMethodes::GeographicMsgs::GeoPoint::toJson(msg, *doc, doc->GetAllocator());
         assert(ret);
         (void)ret;
 
@@ -92,7 +93,7 @@ private:
     rapidjson::Document *_create(const U &msg, Type2Type<MessageGroups::PolygonGroup>){
         using namespace ROSBridge;
         rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
-        bool ret = JsonMethodes::Polygon::toJson(msg, *doc, doc->GetAllocator());
+        bool ret = JsonMethodes::GeometryMsgs::Polygon::toJson(msg, *doc, doc->GetAllocator());
         assert(ret);
         (void)ret;
 
@@ -115,7 +116,7 @@ private:
     rapidjson::Document *_createPolygonStamped(const U &msg, Int2Type<k>){ // U has member header(), use integraded header.
         using namespace ROSBridge;
         rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
-        bool ret = JsonMethodes::PolygonStamped::toJson(msg, *doc, doc->GetAllocator());
+        bool ret = JsonMethodes::GeometryMsgs::PolygonStamped::toJson(msg, *doc, doc->GetAllocator());
         assert(ret);
         (void)ret;
 
@@ -128,7 +129,7 @@ private:
         using namespace ROSBridge;
         JsonMethodes::Header::Header header(HeaderPolicy::header(msg));
         rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
-        bool ret = JsonMethodes::PolygonStamped::toJson(msg, header, *doc, doc->GetAllocator());
+        bool ret = JsonMethodes::GeometryMsgs::PolygonStamped::toJson(msg.polygon(), header, *doc, doc->GetAllocator());
         assert(ret);
         (void)ret;
 
@@ -151,7 +152,7 @@ private:
     rapidjson::Document *_createPolygonArray(const U &msg, Int2Type<k>){ // U has member header(), use integraded header.
         using namespace ROSBridge;
         rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
-        bool ret = JsonMethodes::PolygonArray::toJson(msg, *doc, doc->GetAllocator());
+        bool ret = JsonMethodes::JSKRecognitionMsg::PolygonArray::toJson(msg, *doc, doc->GetAllocator());
         assert(ret);
         (void)ret;
 
@@ -164,17 +165,30 @@ private:
         using namespace ROSBridge;
         JsonMethodes::Header::Header header(HeaderPolicy::header(msg));
         rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
-        bool ret = JsonMethodes::PolygonArray::toJson(msg, header, *doc, doc->GetAllocator());
+        bool ret = JsonMethodes::JSKRecognitionMsg::PolygonArray::toJson(msg, header, *doc, doc->GetAllocator());
         assert(ret);
         (void)ret;
 
         return doc;
     }
 
+    // ===========================================================================
+    // ProgressGroup
+    // ===========================================================================
+    template<class U>
+    rapidjson::Document *_create(const U &msg, Type2Type<MessageGroups::ProgressGroup>){
+        using namespace ROSBridge;
+        rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
+        bool ret = JsonMethodes::NemoMsgs::Progress::toJson(msg, *doc, doc->GetAllocator());
+        assert(ret);
+        (void)ret;
 
+        return doc;
+    }
 };
 
 class StdHeaderPolicy{
+    namespace StdMsgs = ROSBridge::GenericMessages::StdMsgs;
 public:
     StdHeaderPolicy():_seq(-1){};
 
@@ -183,8 +197,8 @@ public:
     //! \return Returns the header belonging to msg.
     //!
     template<typename T>
-        ROSBridge::JsonMethodes::Header::Header header(const T&msg) {
-        return ROSBridge::JsonMethodes::Header::Header(++_seq, time(msg), "/map");
+        StdMsgs::Header header(const T&msg) {
+        return StdMsgs::Header(++_seq, time(msg), "/map");
     }
 
 
@@ -192,9 +206,9 @@ public:
     //! \brief time Returns the current time.
     //! \return Returns the current time.
     template<typename T>
-    ROSBridge::JsonMethodes::Time::Time time(const T&msg) {
+    StdMsgs::Time time(const T&msg) {
         (void)msg;
-        return ROSBridge::JsonMethodes::Time::Time(0,0);
+        return StdMsgs::Time(0,0);
     }
 private:
     long _seq;

src/comm/ros_bridge/include/MessageBaseClass.h

@@ -12,7 +12,6 @@ namespace ROSBridge {
 //! is used by the \class ROSJsonFactory to determine the type of the message it creates. The
 //! MessageBaseClass belongs to the \struct EmptyGroup. Passing a class belonging to the \struct EmptyGroup to the
 //! \class ROSJsonFactory will yield an error.
-template <typename StringType>
 class MessageBaseClass{
 public:
     typedef MessageGroups::EmptyGroup Group;
@@ -25,5 +24,7 @@ public:
 
     virtual MessageBaseClass* Clone() const = 0;
 };
+typedef MessageBaseClass MsgBase;
+} // namespace ROSBridge
+
 
-}

src/comm/ros_bridge/include/MessageGroups.h

@@ -10,12 +10,20 @@ typedef std::string StringType;
 
 template<typename Group, typename SubGroup, typename...MoreSubGroups>
 struct MessageGroup {
-    static StringType label() {return _label<Group, SubGroup, MoreSubGroups...>();}
+    static StringType messageNameFull() {return _full<Group, SubGroup, MoreSubGroups...>();}
 
     template<typename G, typename SubG, typename...MoreSubG>
-    static StringType _label() {return G::label()+ "/" + _label<SubG, MoreSubG...>(); }
+    static StringType _full() {return G::label()+ "/" + _full<SubG, MoreSubG...>(); }
     template<typename G>
-    static StringType _label() {return G::label(); }
+    static StringType _full() {return G::label(); }
+
+
+    static StringType messageNameLast() {return _last<Group, SubGroup, MoreSubGroups...>();}
+
+    template<typename G, typename SubG, typename...MoreSubG>
+    static StringType _last() {return _last<SubG, MoreSubG...>(); }
+    template<typename G>
+    static StringType _last() {return G::label(); }
 };
 
 //!
@@ -76,14 +84,27 @@ struct JSKRecognitionMsgs {
     static StringType label() {return "jsk_recognition_msgs";}
 
     //!
-    //! \brief The PolygonArrayGroup struct is used the mark a class as a ROS PolygonArray message.
+    //! \brief The PolygonArrayGroup struct is used the mark a class as a ROS jsk_recognition_msgs/PolygonArray message.
     //!
-    //! The PolygonArrayGroup struct is used the mark a class as a ROS PolygonArray message.
+    //! The PolygonArrayGroup struct is used the mark a class as a ROS jsk_recognition_msgs/PolygonArray message.
     struct PolygonArrayGroup {
         static StringType label() {return "PolygonArray";}
     };
 };
 
+struct NemoMsgs {
+
+    static StringType label() {return "nemo_msgs";}
+
+    //!
+    //! \brief The ProgressGroup struct is used the mark a class as a ROS nemo_msgs/Progress message.
+    //!
+    //! The ProgressGroup struct is used the mark a class as a ROS nemo_msgs/Progress message.
+    struct ProgressGroup {
+        static StringType label() {return "Progress";}
+    };
+};
+
 
 typedef MessageGroup<detail::EmptyGroup,
                      detail::EmptyGroup>
@@ -109,6 +130,10 @@ typedef MessageGroups::MessageGroup<MessageGroups::JSKRecognitionMsgs,
                                     MessageGroups::JSKRecognitionMsgs::PolygonArrayGroup>
         PolygonArrayGroup;
 
+typedef MessageGroups::MessageGroup<MessageGroups::NemoMsgs,
+                                    MessageGroups::NemoMsgs::ProgressGroup>
+        ProgressGroup;
+
 } // end namespace MessageGroups
 
 } // end namespace ros_bridge

src/comm/ros_bridge/include/PackageBuffer.h

+#pragma once
+#pragma once
+
+#include "boost/lockfree/queue.hpp"
+#include <functional>
+
+namespace ROSBridge {
+namespace Bridge {
+namespace lf = ::boost::lockfree;
+template <class T>
+class PackageBuffer
+{
+public:
+    PackageBuffer();
+
+    void push(T t) {
+        buffer.push(t);
+        if (_pushCallback)
+            _pushCallback();
+    }
+
+    T pop() {
+        T temp = buffer.pop();
+        if (_popCallback)
+            _popCallback();
+        return temp;
+    }
+
+    void setPushCallback(std::function<void(void)> pushCallback) {
+        _pushCallback = pushCallback;
+    }
+
+    void setPopCallback(std::function<void(void)> popCallback) {
+        _popCallback = popCallback;
+    }
+
+    bool empty() const { return buffer.empty();}
+private:
+    lf::queue<T> buffer;
+    std::function<void(void)> _popCallback;
+    std::function<void(void)> _pushCallback;
+};
+
+} // namespace Communicator
+
+} // namespace
+

src/comm/ros_bridge/include/ROSCommunicator.h

 #pragma once
 
-#include "ros_bridge/include/MessageBaseClass.h"
+#include "ros_bridge/rapidjson/include/rapidjson/document.h"
 
-typedef ROSBridge::MessageBaseClass<std::string> ROSMsg;
+#include <memory>
+#include <tuple>
 
-class ROSCommunicator
+#include "boost/lockfree/queue.hpp"
+#include "ros_bridge/include/
+
+namespace ROSBridge {
+namespace lf = ::boost::lockfree;
+class Communicator
 {
+    typedef std::unique_ptr<rapidjson::Document> UniqueJsonPtr;
+    typedef std::tuple<UniqueJsonPtr, std::string> MsgTopicHashPair;
 public:
-  explicit ROSCommunicator() {}
-
-    void send(const ROSMsg *msg);
+  explicit Communicator() {}
 
+    void send(UniqueJsonPtr &msg);
+    void start();
+    void stop();
+    virtual UniqueJsonPtr receive() = 0;
 
-public :
-    virtual void receive(ROSMsg *msg) = 0;
+private:
+    lf::queue<MsgTopicPair> _transmittBuffer;
+    lf::queue<MsgTopicPair> _receiveBuffer;
 };
+}

src/comm/ros_bridge/include/TypeFactory.h

 #pragma once
-#pragma once
 
 #include "ros_bridge/rapidjson/include/rapidjson/document.h"
 #include "ros_bridge/include/JsonMethodes.h"
@@ -19,10 +18,9 @@ namespace ROSBridge {
 //!
 //! The TypeFactory class is used to create C++ representatives of ROS messages
 //! (classes derived from \class MessageBaseClass) from a \class rapidjson::Document document.
-template <class StringType = std::string>
 class TypeFactory
 {
-    typedef MessageBaseClass<StringType> ROSMsg;
+    typedef MessageBaseClass ROSMsg;
 public:
 
     TypeFactory(){}
@@ -61,7 +59,7 @@ private:
     template<class U>
     bool _create(const rapidjson::Document &doc, U &type, Type2Type<MessageGroups::Point32Group>){
         using namespace ROSBridge;
-        bool ret = JsonMethodes::Point32::fromJson(doc, type);
+        bool ret = JsonMethodes::GeometryMsgs::Point32::fromJson(doc, type);
         assert(ret);
         return ret;
     }
@@ -72,7 +70,7 @@ private:
     template<class U>
     bool _create(const rapidjson::Document &doc, U &type, Type2Type<MessageGroups::GeoPointGroup>){
         using namespace ROSBridge;
-        bool ret = JsonMethodes::GeoPoint::fromJson(doc, type);
+        bool ret = JsonMethodes::GeographicMsgs::GeoPoint::fromJson(doc, type);
         assert(ret);
         return ret;
     }
@@ -83,7 +81,7 @@ private:
     template<class U>
     bool _create(const rapidjson::Document &doc, U &type, Type2Type<MessageGroups::PolygonGroup>){
         using namespace ROSBridge;
-        bool ret = JsonMethodes::Polygon::fromJson(doc, type);
+        bool ret = JsonMethodes::GeometryMsgs::Polygon::fromJson(doc, type);
         assert(ret);
         return ret;
     }
@@ -94,7 +92,7 @@ private:
     template<class U>
     bool _create(const rapidjson::Document &doc, U &type, Type2Type<MessageGroups::PolygonStampedGroup>){
         using namespace ROSBridge;
-        bool ret = JsonMethodes::PolygonStamped::fromJson(doc, type);
+        bool ret = JsonMethodes::GeometryMsgs::PolygonStamped::fromJson(doc, type);
         assert(ret);
         return ret;
     }
@@ -105,7 +103,18 @@ private:
     template<class U>
     bool _create(const rapidjson::Document &doc, U &type, Type2Type<MessageGroups::PolygonArrayGroup>){
         using namespace ROSBridge;
-        bool ret = JsonMethodes::PolygonArray::fromJson(doc, type);
+        bool ret = JsonMethodes::JSKRecognitionMsgs::PolygonArray::fromJson(doc, type);
+        assert(ret);
+        return ret;
+    }
+
+    // ===========================================================================
+    // ProgressGroup
+    // ===========================================================================
+    template<class U>
+    bool _create(const rapidjson::Document &doc, U &type, Type2Type<MessageGroups::ProgressGroup>){
+        using namespace ROSBridge;
+        bool ret = JsonMethodes::NemoMsgs::Progress::fromJson(doc, type);
         assert(ret);
         return ret;
     }

src/comm/ros_bridge/src/CasePacker.cpp

+#include "CasePacker.h"
+
+const char* ROSBridge::CasePacker::topicKey         = "topic";
+const char* ROSBridge::CasePacker::messageTypeKey   = "messageType";
+
+ROSBridge::CasePacker::CasePacker()
+{
+
+}
+
+void ROSBridge::CasePacker::_addTag(JsonDoc &doc, const char *topic, const char *messageType)
+{
+    using namespace ROSBridge;
+    using namespace rapidjson;
+
+    {
+    // add topic
+    rapidjson::Value key(CasePacker::topicKey, doc.GetAllocator());
+    rapidjson::Value value(topic, doc.GetAllocator());
+    doc.AddMember(key, value, doc.GetAllocator());
+    }
+
+    // add messageType
+    rapidjson::Value key(CasePacker::messageTypeKey, doc.GetAllocator());
+    rapidjson::Value value(messageType, doc.GetAllocator());
+    doc.AddMember(key, value, doc.GetAllocator());
+}
+
+void ROSBridge::CasePacker::_removeTag(JsonDoc &doc)
+{
+    using namespace ROSBridge;
+    using namespace rapidjson;
+    if ( doc.HasMember(CasePacker::topicKey) )
+        doc.RemoveMember(CasePacker::topicKey);
+    if ( doc.HasMember(CasePacker::messageTypeKey) )
+        doc.RemoveMember(CasePacker::messageTypeKey);
+}
+
+

src/comm/ros_bridge/src/CasePacker.h

+#pragma once
+#include "ros_bridge/include/MessageBaseClass.h"
+
+#include <memory>
+#include "rapidjson/include/rapidjson/document.h"
+
+namespace ROSBridge {
+
+class CasePacker
+{
+    typedef rapidjson::Document JsonDoc;
+    typedef std::unique_ptr<JsonDoc> UniqueJsonPtr;
+public:
+    CasePacker();
+
+    struct MessageTag {
+       char *topic;
+       char *messagType;
+    };
+    typedef MessageTag Tag;
+
+
+    template<class T>
+    void packAndSend(const T &msg, const char *topic);
+
+    const Tag &showTag();
+
+    template<class T>
+    void unpack(T &msg);
+
+protected:
+    void _addTag(JsonDoc &doc, const char *topic, const char *messageType);
+    void _removeTag(JsonDoc &doc);
+
+    static const char* topicKey;
+    static const char* messageTypeKey;
+
+private:
+    Tag _tag;
+};
+}
+

src/comm/ros_bridge/src/PackageBuffer.h

+#pragma once
+
+#include "boost/lockfree/queue.hpp"
+#include <functional>
+
+namespace ROSBridge {
+namespace Bridge {
+namespace lf = ::boost::lockfree;
+template <class T>
+class PackageBuffer
+{
+public:
+    PackageBuffer();
+
+    void push(T t) {
+        buffer.push(t);
+        if (_pushCallback)
+            _pushCallback();
+    }
+
+    T pop() {
+        T temp = buffer.pop();
+        if (_popCallback)
+            _popCallback();
+        return temp;
+    }
+
+    void setPushCallback(std::function<void(void)> pushCallback) {
+        _pushCallback = pushCallback;
+    }
+
+    void setPopCallback(std::function<void(void)> popCallback) {
+        _popCallback = popCallback;
+    }
+
+    bool empty() const { return buffer.empty();}
+private:
+    lf::queue<T> buffer;
+    std::function<void(void)> _popCallback;
+    std::function<void(void)> _pushCallback;
+};
+
+} // namespace Communicator
+
+} // namespace

src/comm/ros_bridge/src/ROSCommunicator.cpp

 #include "ros_bridge/include/ROSCommunicator.h"
+
+void ROSBridge::Communicator::send(ROSBridge::Communicator::UniqueJsonPtr &msg)
+{
+    if (!msg->HasMember(""))
+}