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/GenericMessages.h

+#pragma once
+#include <iostream>
+#include <vector>
+
+#include "boost/type_traits/remove_cv_ref.hpp"
+#include "ros_bridge/include/MessageBaseClass.h"
+
+namespace ROSBridge {
+//!@brief Namespace containing ROS message generics.
+namespace GenericMessages {
+//!@brief Namespace containing ROS std_msgs generics.
+
+typedef std::ostream OStream;
+namespace StdMsgs {
+
+//! @brief C++ representation of std_msgs/Time
+class Time{
+public:
+    Time(): _secs(0), _nsecs(0) {}
+    Time(uint32_t secs, uint32_t nsecs): _secs(secs), _nsecs(nsecs) {}
+
+    uint32_t secs()  const {return _secs;}
+    uint32_t nSecs() const {return _nsecs;}
+
+    void setSecs(uint32_t secs)     {_secs = secs;}
+    void setNSecs(uint32_t nsecs)   {_nsecs = nsecs;}
+
+private:
+    uint32_t _secs;
+    uint32_t _nsecs;
+};
+
+
+//! @brief C++ representation of std_msgs/Header
+class Header{
+public:
+    Header() : _seq(0), _stamp(Time()), _frameId("") {}
+    Header(uint32_t seq, const Time &stamp, const std::string &frame_id) :
+        _seq(seq)
+      , _stamp(stamp)
+      , _frameId(frame_id) {}
+
+    uint32_t            seq()       const {return _seq;};
+    const Time   &stamp()     const {return _stamp;};
+    const std::string  &frameId()   const {return _frameId;};
+
+    Time   &stamp()         {return _stamp;};
+    std::string  &frameId()       {return _frameId;};
+
+    void setSeq     (uint32_t seq)                  {_seq = seq;}
+    void setStamp   (const Time &stamp)       {_stamp = stamp;}
+    void setFrameId (const std::string  &frameId)   {_frameId = frameId;}
+private:
+    uint32_t    _seq;
+    Time        _stamp;
+    std::string _frameId;
+};
+} //StdMsgs
+//!@brief Namespace containing ROS geometry_msgs generics.
+namespace GeometryMsgs {
+
+// ==============================================================================
+// class GenericPoint
+// ==============================================================================
+
+//! @brief C++ representation of a geometry_msgs/Point32
+template<typename FloatType = _Float64>
+class GenericPoint : public ROSBridge::MessageBaseClass {
+public:
+    typedef ROSBridge::MessageGroups::Point32Group Group;
+    GenericPoint() : ROSBridge::MessageBaseClass(){}
+    GenericPoint(FloatType x, FloatType y, FloatType z) : ROSBridge::MessageBaseClass(), _x(x), _y(y), _z(z){}
+
+    FloatType x() const {return _x;}
+    FloatType y() const {return _y;}
+    FloatType z() const {return _z;}
+
+    void setX(FloatType x) {_x = x;}
+    void setY(FloatType y) {_y = y;}
+    void setZ(FloatType z) {_z = z;}
+
+
+
+    bool operator==(const GenericPoint &p1) {
+        return (p1._x == this->_x
+                && p1._y == this->_y
+                && p1._z == this->_z);
+    }
+
+    bool operator!=(const GenericPoint &p1) {
+        return !this->operator==(p1);
+    }
+
+    friend OStream& operator<<(OStream& os, const GenericPoint& p)
+    {
+        os << "x: " << p._x << " y: " << p._y<< " z: " << p._z;
+        return os;
+    }
+
+    GenericPoint *Clone() const override {return new GenericPoint(*this);}
+
+private:
+    FloatType _x;
+    FloatType _y;
+    FloatType _z;
+};
+
+typedef GenericPoint<> Point;
+typedef GenericPoint<_Float32> Point32;
+
+
+// ==============================================================================
+// class GenericPolygon
+// ==============================================================================
+
+//! @brief C++ representation of geometry_msgs/Polygon
+template <class PointTypeCVR,
+          template <class, class...> class ContainerType = std::vector>
+class GenericPolygon : public ROSBridge::MessageBaseClass {
+    typedef typename boost::remove_cv_ref_t<PointTypeCVR> PointType;
+    typedef ROSBridge::MessageBaseClass Base;
+public:
+    typedef ROSBridge::MessageGroups::PolygonGroup Group;
+    GenericPolygon() : Base() {}
+    GenericPolygon(const GenericPolygon &poly) : Base(), _points(poly.points()){}
+
+
+    const ContainerType<PointType> &points() const {return _points;}
+          ContainerType<PointType> &points()       {return _points;}
+
+          GenericPolygon *Clone() const override { return new GenericPolygon(*this);}
+
+private:
+    ContainerType<PointType> _points;
+};
+
+typedef GenericPolygon<Point> Polygon;
+
+
+// ==============================================================================
+// class GenericPolygonStamped
+// ==============================================================================
+using namespace ROSBridge::GenericMessages::StdMsgs;
+
+//! @brief C++ representation of geometry_msgs/PolygonStamped
+template <class PolygonType  = Polygon,
+          class HeaderType = Header>
+class GenericPolygonStamped : public ROSBridge::MessageBaseClass{
+    typedef PolygonType Polygon;
+    typedef ROSBridge::MessageBaseClass Base;
+public:
+    typedef ROSBridge::MessageGroups::PolygonStampedGroup Group;
+    GenericPolygonStamped() : Base() {};
+    GenericPolygonStamped(const GenericPolygonStamped &other) :
+         Base()
+      , _header(other.header())
+      , _polygon(other.polygon())
+    {}
+    GenericPolygonStamped(const HeaderType &header, Polygon &polygon) :
+         Base()
+      , _header(header)
+      , _polygon(polygon)
+    {}
+
+
+    const HeaderType  &header()  const   {return _header;}
+    const Polygon     &polygon() const   {return _polygon;}
+
+    HeaderType  &header()  {return _header;}
+    Polygon     &polygon() {return _polygon;}
+
+    GenericPolygonStamped *Clone() const override {return new GenericPolygonStamped(*this);}
+
+private:
+    HeaderType   _header;
+    Polygon      _polygon;
+};
+
+typedef GenericPolygonStamped<> PolygonStamped;
+
+} // namespace GeometryMsgs
+//!@brief Namespace containing ROS geographic_msgs generics.
+namespace GeographicMsgs {
+
+
+// ==============================================================================
+// class GenericGeoPoint
+// ==============================================================================
+
+//! @brief C++ representation of geographic_msgs/GeoPoint.
+class GeoPoint : public ROSBridge::MessageBaseClass{
+    typedef ROSBridge::MessageBaseClass Base;
+public:
+    typedef ROSBridge::MessageGroups::GeoPointGroup Group;
+    GeoPoint()
+        : Base()
+        , _latitude(0)
+        , _longitude(0)
+        , _altitude(0)
+    {}
+    GeoPoint(const GeoPoint &other)
+        : Base()
+        , _latitude(other.latitude())
+        , _longitude(other.longitude())
+        , _altitude(other.altitude())
+    {}
+    GeoPoint(_Float64 latitude, _Float64 longitude, _Float64 altitude)
+        : Base()
+        , _latitude(latitude)
+        , _longitude(longitude)
+        , _altitude(altitude)
+    {}
+
+    _Float64 latitude()     const {return _latitude;}
+    _Float64 longitude()    const {return _longitude;}
+    _Float64 altitude()     const {return _altitude;}
+
+    void setLatitude    (_Float64 latitude)    {_latitude  = latitude;}
+    void setLongitude   (_Float64 longitude)   {_longitude = longitude;}
+    void setAltitude    (_Float64 altitude)    {_altitude  = altitude;}
+
+    GeoPoint *Clone() const override {return new GeoPoint(*this);}
+
+    bool operator==(const GeoPoint &p1) {
+        return (p1._latitude == this->_latitude
+                && p1._longitude == this->_longitude
+                && p1._altitude == this->_altitude);
+    }
+
+    bool operator!=(const GeoPoint &p1) {
+        return !this->operator==(p1);
+    }
+
+    friend OStream& operator<<(OStream& os, const GeoPoint& p)
+    {
+        os << "lat: " << p._latitude << " lon: " << p._longitude<< " alt: " << p._altitude;
+        return os;
+    }
+
+private:
+    _Float64 _latitude;
+    _Float64 _longitude;
+    _Float64 _altitude;
+};
+
+} // namespace GeographicMsgs
+//!@brief Namespace containing ROS jsk_recognition_msgs generics.
+namespace JSKRecognitionMsgs {
+using namespace ROSBridge::GenericMessages::StdMsgs;
+using namespace ROSBridge::GenericMessages::GeometryMsgs;
+
+// ==============================================================================
+// class GenericPolygonArray
+// ==============================================================================
+
+//! @brief C++ representation of jsk_recognition_msgs/PolygonArray
+template <class PolygonType = Polygon,
+          template <class, class...> class ContainerType = std::vector,
+          class HeaderType =  Header,
+          class IntType = long,
+          class FloatType = double>
+class GenericPolygonArray : ROSBridge::MessageBaseClass{
+    typedef ROSBridge::MessageBaseClass Base;
+public:
+    typedef ROSBridge::MessageGroups::PolygonArrayGroup Group;
+    GenericPolygonArray() : Base() {}
+    GenericPolygonArray(const GenericPolygonArray &other)
+      : Base()
+      , _header(other.header())
+      , _polygons(other.polygons())
+      , _labels(other.labels())
+      , _likelihood(other.likelihood())
+    {}
+    GenericPolygonArray(const HeaderType                   &header,
+                 const ContainerType<PolygonType>   &polygons,
+                 const ContainerType<IntType>      &labels,
+                 const ContainerType<FloatType>      &likelihood)
+      : Base()
+      , _header(header)
+      , _polygons(polygons)
+      , _labels(labels)
+      , _likelihood(likelihood)
+    {}
+
+    const HeaderType                 &header()     const {return _header;}
+          HeaderType                 &header()           {return _header;}
+    const ContainerType<PolygonType> &polygons()   const {return _polygons;}
+          ContainerType<PolygonType> &polygons()         {return _polygons;}
+    const ContainerType<IntType>     &labels()     const {return _labels;}
+          ContainerType<IntType>     &labels()           {return _labels;}
+    const ContainerType<FloatType>   &likelyhood() const {return _likelihood;}
+          ContainerType<FloatType>   &likelyhood()       {return _likelihood;}
+
+          GenericPolygonArray *Clone() const override {return new GenericPolygonArray(*this);}
+
+
+private:
+    HeaderType                  _header;
+    ContainerType<PolygonType>  _polygons;
+    ContainerType<IntType>     _labels;
+    ContainerType<FloatType>     _likelihood;
+};
+
+typedef GenericPolygonArray<> PolygonArray;
+
+} // namespace JSKRecognitionMsgs
+//!@brief Namespace containing ROS nemo_msgs generics.
+namespace NemoMsgs {
+
+// ==============================================================================
+// class GenericProgress
+// ==============================================================================
+
+//! @brief C++ representation of nemo_msgs/Progress message
+template <class IntType = long, template <class, class...> class ContainterType = std::vector>
+class GenericProgress {
+public:
+    GenericProgress(){}
+    GenericProgress(const ContainterType<IntType> &progress) :_progress(progress){}
+
+    const ContainterType<IntType> &progress(void) const {return _progress;}
+          ContainterType<IntType> &progress(void)       {return _progress;}
+private:
+    ContainterType<IntType> _progress;
+};
+
+typedef GenericProgress<> Progress;
+
+} // namespace NemoMsgs
+} // namespace GenericMessages
+} // namespace ROSBridge

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/JsonMethodes.h

@@ -14,111 +14,62 @@
 #include <ostream>
 
 namespace ROSBridge {
-namespace JsonMethodes {
 
-typedef std::ostream OStream;
+//! @brief Namespace containing methodes for Json generation.
+namespace JsonMethodes {
 
+//! @brief Namespace containing methodes for std_msgs generation.
+namespace StdMsgs {
+//! @brief Namespace containing methodes for std_msgs/Time message generation.
 namespace Time {    
-    //! @brief C++ representation of std_msgs/Time
-    class Time{
-    public:
-        Time(): _secs(0), _nsecs(0) {}
-        Time(uint32_t secs, uint32_t nsecs): _secs(secs), _nsecs(nsecs) {}
-
-        uint32_t secs()  const {return _secs;}
-        uint32_t nSecs() const {return _nsecs;}
-
-        void setSecs(uint32_t secs)     {_secs = secs;}
-        void setNSecs(uint32_t nsecs)   {_nsecs = nsecs;}
-
-    private:
-        uint32_t _secs;
-        uint32_t _nsecs;
-    };
-
     template<class TimeType>
     bool toJson(const TimeType &time,
-                rapidjson::Document &doc,
+                rapidjson::Value &value,
                 rapidjson::Document::AllocatorType &allocator)
     {
-        doc.AddMember("secs", rapidjson::Value().SetUint((uint32_t)time.secs()), allocator);
-        doc.AddMember("nsecs", rapidjson::Value().SetUint((uint32_t)time.nSecs()), allocator);
+        value.AddMember("secs", rapidjson::Value().SetUint((uint32_t)time.secs()), allocator);
+        value.AddMember("nsecs", rapidjson::Value().SetUint((uint32_t)time.nSecs()), allocator);
 
         return true;
     }
 
-    template<class TimeType, class JsonType>
-    bool _fromJson(const JsonType &doc,
+    template<class TimeType>
+    bool fromJson(const rapidjson::Value &value,
                   TimeType &time)
     {
-        if (!doc.HasMember("secs") || !doc["secs"].IsUint())
-            return false;
-        if (!doc.HasMember("nsecs")|| !doc["nsecs"].IsUint())
+        if (!value.HasMember("secs") || !value["secs"].IsUint()){
+            assert(false);
             return false;
-        time.setSecs(doc["secs"].GetUint());
-        time.setNSecs(doc["nsecs"].GetUint());
-
-        return true;
         }
-
-    template<class TimeType>
-    bool fromJson(const rapidjson::Document &doc,
-                  TimeType &time)
-    {
-        return _fromJson(doc, time);
+        if (!value.HasMember("nsecs")|| !value["nsecs"].IsUint()){
+            assert(false);
+            return false;
         }
+        time.setSecs(value["secs"].GetUint());
+        time.setNSecs(value["nsecs"].GetUint());
 
-    template<class TimeType>
-    bool fromJson(const rapidjson::Value &doc,
-                  TimeType &time)
-    {
-        return _fromJson(doc, time);
+        return true;
     }
 
-}
+} // Time
 
+//! @brief Namespace containing methodes for std_msgs/Header message generation.
 namespace Header {
 
-
-
-    //! @brief C++ representation of std_msgs/Header
-    class Header{
-    public:
-        Header() : _seq(0), _stamp(Time::Time()), _frameId("") {}
-        Header(uint32_t seq, const Time::Time &stamp, const std::string &frame_id) :
-            _seq(seq)
-          , _stamp(stamp)
-          , _frameId(frame_id) {}
-
-        uint32_t            seq()       const {return _seq;};
-        const Time::Time   &stamp()     const {return _stamp;};
-        const std::string  &frameId()   const {return _frameId;};
-
-        Time::Time   &stamp()         {return _stamp;};
-        std::string  &frameId()       {return _frameId;};
-
-        void setSeq     (uint32_t seq)                  {_seq = seq;}
-        void setStamp   (const Time::Time &stamp)       {_stamp = stamp;}
-        void setFrameId (const std::string  &frameId)   {_frameId = frameId;}
-    private:
-        uint32_t    _seq;
-        Time::Time        _stamp;
-        std::string _frameId;
-    };
-
-
     template <class HeaderType>
     bool toJson(const HeaderType &header,
-                rapidjson::Document &doc,
+                rapidjson::Value &value,
                 rapidjson::Document::AllocatorType &allocator)     {
-        doc.AddMember("seq", rapidjson::Value().SetUint((uint32_t)header.seq()), allocator);
+        value.AddMember("seq", rapidjson::Value().SetUint((uint32_t)header.seq()), allocator);
 
-        rapidjson::Document stamp(rapidjson::kObjectType);
-        if (!Time::toJson(header.stamp(), doc, allocator))
+        rapidjson::Value stamp(rapidjson::kObjectType);
+        if (!Time::toJson(header.stamp(), stamp, allocator)){
+            assert(false);
             return false;
-        doc.AddMember("stamp", stamp, allocator);
+        }
+        value.AddMember("stamp", stamp, allocator);
 
-        doc.AddMember("frame_id",
+        value.AddMember("frame_id",
                       rapidjson::Value().SetString(header.frameId().data(),
                                                    header.frameId().length(),
                                                    allocator),
@@ -127,69 +78,43 @@ namespace Header {
         return true;
     }
 
-    template <class HeaderType, class JsonType>
-    bool _fromJson(const JsonType &doc,
+    template <class HeaderType>
+    bool fromJson(const rapidjson::Value &value,
                   HeaderType &header)     {
-        if (!doc.HasMember("seq")|| !doc["seq"].IsUint())
+        if (!value.HasMember("seq")|| !value["seq"].IsUint()){
+            assert(false);
             return false;
-        if (!doc.HasMember("stamp"))
+        }
+        if (!value.HasMember("stamp")){
+            assert(false);
             return false;
-        if (!doc.HasMember("frame_id")|| !doc["frame_id"].IsString())
+        }
+        if (!value.HasMember("frame_id")|| !value["frame_id"].IsString()){
+            assert(false);
             return false;
-        header.setSeq(doc["seq"].GetUint());
+        }
+        header.setSeq(value["seq"].GetUint());
         decltype(header.stamp()) time;
-        if (!Time::fromJson(doc["stamp"], time))
+        if (!Time::fromJson(value["stamp"], time)){
+            assert(false);
             return false;
+        }
         header.setStamp(time);
-        header.setFrameId(doc["frame_id"].GetString());
+        header.setFrameId(value["frame_id"].GetString());
 
         return true;
     }
 
+} // Header
+} // StdMsgs
 
-    template<class HeaderType>
-    bool fromJson(const rapidjson::Value &doc,
-                  HeaderType &header)
-    {
-        return _fromJson(doc, header);
-    }
-
-    template<class HeaderType>
-    bool fromJson(const rapidjson::Document &doc,
-                  HeaderType &header)
-    {
-        return _fromJson(doc, header);
-    }
-
-}
 
+//! @brief Namespace containing methodes for geometry_msgs generation.
+namespace GeometryMsgs {
+//! @brief Namespace containing methodes for geometry_msgs/Point32 message generation.
 namespace Point32 {
 using namespace ROSBridge::traits;
 
-        //! @brief C++ representation of geometry_msgs/Point32.
-        template<typename FloatType = _Float64>
-        class Point{
-        public:
-            Point(FloatType x, FloatType y, FloatType z) : _x(x), _y(y), _z(z){}
-
-            FloatType x() const {return _x;}
-            FloatType y() const {return _y;}
-            FloatType z() const {return _z;}
-
-            void setX(FloatType x) {_x = x;}
-            void setY(FloatType y) {_y = y;}
-            void setZ(FloatType z) {_z = z;}
-
-
-        private:
-            FloatType _x;
-            FloatType _y;
-            FloatType _z;
-        };
-
-        typedef Point<> Point64;
-        typedef Point<_Float32> Point32;
-
         namespace det { //detail
 
             template <class T>
@@ -222,104 +147,177 @@ using namespace ROSBridge::traits;
         }
 
         template <class T>
-        bool toJson(const T&p, rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
+        bool toJson(const T&p, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
         {
-            doc.AddMember("x", rapidjson::Value().SetFloat(p.x()), allocator);
-            doc.AddMember("y", rapidjson::Value().SetFloat(p.y()), allocator);
+            value.AddMember("x", rapidjson::Value().SetFloat(p.x()), allocator);
+            value.AddMember("y", rapidjson::Value().SetFloat(p.y()), allocator);
 
             typedef typename Select<HasMemberZ<T>::value, Has3Components, Has2Components>::Result
                     Components; // Check if PointType has 2 or 3 dimensions.
             auto z = det::getZ(p, Type2Type<Components>()); // If T has no member z() replace it by 0.
 
-            doc.AddMember("y", rapidjson::Value().SetFloat(z), allocator);
+            value.AddMember("z", rapidjson::Value().SetFloat(z), allocator);
 
             return true;
         }
 
-        template <class PointType, class JsonType>
-        bool _fromJson(const JsonType &doc,
+        template <class PointType>
+        bool fromJson(const rapidjson::Value &value,
                       PointType &p)     {
-            if (!doc.HasMember("x") || !doc["x"].IsFloat())
+            if (!value.HasMember("x") || !value["x"].IsFloat()){
+                assert(false);
                 return false;
-            if (!doc.HasMember("y") || !doc["y"].IsFloat())
+            }
+            if (!value.HasMember("y") || !value["y"].IsFloat()){
+                assert(false);
                 return false;
-            if (!doc.HasMember("z") || !doc["z"].IsFloat())
+            }
+            if (!value.HasMember("z") || !value["z"].IsFloat()){
+                assert(false);
                 return false;
+            }
 
-            p.setX(doc["x"].GetFloat());
-            p.setY(doc["y"].GetFloat());
+            p.setX(value["x"].GetFloat());
+            p.setY(value["y"].GetFloat());
             typedef typename Select<HasMemberSetZ<PointType>::value, Has3Components, Has2Components>::Result
                     Components; // Check if PointType has 2 or 3 dimensions.
-            (void)det::setZ(doc["z"], p,  Type2Type<Components>()); // If PointType has no member z() discard doc["z"].
+            (void)det::setZ(value["z"], p,  Type2Type<Components>()); // If PointType has no member z() discard doc["z"].
 
             return true;
         }
 
+} // Point32
 
-        template<class PointType>
-        bool fromJson(const rapidjson::Value &doc,
-                      PointType &point)
-        {
-            return _fromJson(doc, point);
-        }
+//! @brief Namespace containing methodes for geometry_msgs/Polygon message generation.
+namespace Polygon {
 
-        template<class PointType>
-        bool fromJson(const rapidjson::Document &doc,
-                      PointType &point)
-        {
-            return _fromJson(doc, point);
+template <class PolygonType>
+bool toJson(const PolygonType &poly, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
+{
+    rapidjson::Value points(rapidjson::kArrayType);
+
+    for(unsigned long i=0; i < (unsigned long)poly.points().size(); ++i) {
+        rapidjson::Document point(rapidjson::kObjectType);
+        if ( !Point32::toJson(poly.points()[i], point, allocator) ){
+            assert(false);
+            return false;
         }
+        points.PushBack(point, allocator);
+    }
+
+    value.AddMember("points", points, allocator);
 
+    return true;
 }
 
-namespace GeoPoint {
-using namespace ROSBridge::traits;
+template <class PolygonType>
+bool fromJson(const rapidjson::Value &value, PolygonType &poly)
+{
+    if (!value.HasMember("points") || !value["points"].IsArray()){
+        assert(false);
+        return false;
+    }
+    const auto &jsonArray = value["points"].GetArray();
+    poly.points().reserve(jsonArray.Size());
+    typedef decltype (poly.points()[0]) PointTypeCVR;
+    typedef typename boost::remove_cv<typename boost::remove_reference<PointTypeCVR>::type>::type PointType;
+    for (long i=0; i < jsonArray.Size(); ++i){
+        PointType pt;
+        if ( !Point32::fromJson(jsonArray[i], pt) ){
+            assert(false);
+            return false;
+        }
+        poly.points().push_back(std::move(pt));
+    }
+    return true;
+}
+} // namespace Polygon
 
-        //! @brief C++ representation of geographic_msgs/GeoPoint.
-        class GeoPoint{
-        public:
-            GeoPoint()
-                : _latitude(0)
-                , _longitude(0)
-                , _altitude(0)
-            {}
-            GeoPoint(_Float64 latitude, _Float64 longitude, _Float64 altitude)
-                : _latitude(latitude)
-                , _longitude(longitude)
-                , _altitude(altitude)
-            {}
 
-            _Float64 latitude()     const {return _latitude;}
-            _Float64 longitude()    const {return _longitude;}
-            _Float64 altitude()     const {return _altitude;}
+//! @brief Namespace containing methodes for geometry_msgs/PolygonStamped message generation.
+namespace PolygonStamped {
+using namespace ROSBridge::JsonMethodes::StdMsgs;
 
-            void setLatitude    (_Float64 latitude)    {_latitude  = latitude;}
-            void setLongitude   (_Float64 longitude)   {_longitude = longitude;}
-            void setAltitude    (_Float64 altitude)    {_altitude  = altitude;}
 
+template <class PolyStamped>
+bool toJson(const PolyStamped &polyStamped, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
+{
+    return toJson(polyStamped.polygon(), polyStamped.header(), value, allocator);
+}
 
+template <class PolygonType, class HeaderType>
+bool toJson(const PolygonType &poly, const HeaderType &h, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
+{
+    rapidjson::Document header(rapidjson::kObjectType);
+    if (!Header::toJson(h, header, allocator)){
+        assert(false);
+        return false;
+    }
+    rapidjson::Document polygon(rapidjson::kObjectType);
+    if (!Polygon::toJson(poly, polygon, allocator)){
+        assert(false);
+        return false;
+    }
+    value.AddMember("header", header, allocator);
+    value.AddMember("polygon", polygon, allocator);
+
+    return true;
+}
+
+namespace det {
+template <class PolygonStampedType>
+bool setHeader(const rapidjson::Value &doc, PolygonStampedType &polyStamped, Int2Type<1>) { // polyStamped.setHeader() exists
+    typedef decltype (polyStamped.header()) HeaderTypeCVR;
+    typedef typename boost::remove_cv<typename boost::remove_reference<HeaderTypeCVR>::type>::type HeaderType;
+    HeaderType header;
+    bool ret = Header::fromJson(doc, header);
+    polyStamped.header() = header;
+    return ret;
+}
+template <class PolygonStampedType>
+bool setHeader(const rapidjson::Value &doc, PolygonStampedType &polyStamped, Int2Type<0>) { // polyStamped.setHeader() does not exists
+    (void)doc;
+    (void)polyStamped;
+    return true;
+}
+} // namespace det
 
-            bool operator==(const GeoPoint &p1) {
-                return (p1._latitude == this->_latitude
-                        && p1._longitude == this->_longitude
-                        && p1._altitude == this->_altitude);
+template <class PolygonType, class HeaderType>
+bool fromJson(const rapidjson::Value &value, PolygonType &polyStamped)
+{
+    if ( !value.HasMember("header") ){
+        assert(false);
+        return false;
+    }
+    if ( !value.HasMember("polygon") ){
+        assert(false);
+        return false;
     }
 
-            bool operator!=(const GeoPoint &p1) {
-                return !this->operator==(p1);
+
+    typedef traits::HasMemberSetHeader<PolygonType> HasHeader;
+    if ( !det::setHeader(value["header"], polyStamped, Int2Type<HasHeader::value>())){
+        assert(false);
+        return false;
     }
 
-            friend std::ostream& operator<<(std::ostream& os, const GeoPoint& p)
-            {
-                os << "lat: " << p._latitude << " lon: " << p._longitude<< " alt: " << p._altitude << "\n";
-                return os;
+    if ( !Polygon::fromJson(value["polygon"], polyStamped.polygon()) ){
+        assert(false);
+        return false;
     }
 
-        private:
-            _Float64 _latitude;
-            _Float64 _longitude;
-            _Float64 _altitude;
-        };
+    return true;
+}
+} // namespace PolygonStamped
+
+} // namespace GeometryMsgs
+
+
+//! @brief Namespace containing methodes for geographic_msgs generation.
+namespace GeographicMsgs {
+//! @brief Namespace containing methodes for geographic_msgs/GeoPoint message generation.
+namespace GeoPoint {
+using namespace ROSBridge::traits;
 
         namespace det { //detail
 
@@ -348,279 +346,60 @@ using namespace ROSBridge::traits;
                 (void)doc;
                 (void)p;
             }
-        }
+        } // namespace det
 
         template <class T>
-        bool toJson(const T&p, rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
+        bool toJson(const T&p, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
         {
-            doc.AddMember("latitude", rapidjson::Value().SetFloat((_Float64)p.latitude()), allocator);
-            doc.AddMember("longitude", rapidjson::Value().SetFloat((_Float64)p.longitude()), allocator);
+            value.AddMember("latitude", rapidjson::Value().SetFloat((_Float64)p.latitude()), allocator);
+            value.AddMember("longitude", rapidjson::Value().SetFloat((_Float64)p.longitude()), allocator);
 
             typedef typename Select<HasMemberAltitude<T>::value, Has3Components, Has2Components>::Result
                     Components; // Check if PointType has 2 or 3 dimensions.
             auto altitude = det::getAltitude(p, Type2Type<Components>()); // If T has no member altitude() replace it by 0.0;
 
-            doc.AddMember("altitude", rapidjson::Value().SetFloat((_Float64)altitude), allocator);
+            value.AddMember("altitude", rapidjson::Value().SetFloat((_Float64)altitude), allocator);
             return true;
         }
 
 
 
-        template <class PointType, class JsonType>
-        bool _fromJson(const JsonType &doc,
+        template <class PointType>
+        bool fromJson(const rapidjson::Value &value,
                       PointType &p)     {
-            if (!doc.HasMember("latitude") || !doc["latitude"].IsFloat())
-                return false;
-            if (!doc.HasMember("longitude") || !doc["longitude"].IsFloat())
-                return false;
-            if (!doc.HasMember("altitude") || !doc["altitude"].IsFloat())
+            if (!value.HasMember("latitude") || !value["latitude"].IsFloat()){
+                assert(false);
                 return false;
-
-            p.setLatitude(doc["latitude"].GetFloat());
-            p.setLongitude(doc["longitude"].GetFloat());
-            typedef typename Select<HasMemberSetAltitude<PointType>::value, Has3Components, Has2Components>::Result
-                    Components; // Check if PointType has 2 or 3 dimensions.
-            det::setAltitude(doc["altitude"], p,  Type2Type<Components>()); // If T has no member altitude() discard doc["altitude"];
-
-            return true;
             }
-
-        template<class PointType>
-        bool fromJson(const rapidjson::Value &doc,
-                      PointType &point)
-        {
-            return _fromJson(doc, point);
-        }
-
-        template<class PointType>
-        bool fromJson(const rapidjson::Document &doc,
-                      PointType &point)
-        {
-            return _fromJson(doc, point);
-        }
-
-}
-
-namespace Polygon {
-
-    //! @brief C++ representation of geometry_msgs/Polygon
-    template <class PointTypeCVR,
-              template <class, class...> class ContainerType =  std::vector>
-    class Polygon{
-        typedef typename boost::remove_cv<typename boost::remove_reference<PointTypeCVR>::type>::type PointType;
-    public:
-        Polygon(){}
-        Polygon(const Polygon &poly) : _points(poly.points()){}
-
-
-        const ContainerType<PointType> &points() const {return _points;}
-              ContainerType<PointType> &points()       {return _points;}
-
-    private:
-        ContainerType<PointType> _points;
-    };
-
-    template <class PolygonType>
-    bool toJson(const PolygonType &poly, rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
-    {
-        rapidjson::Value points(rapidjson::kArrayType);
-
-        for(unsigned long i=0; i < (unsigned long)poly.points().size(); ++i) {
-            rapidjson::Document point(rapidjson::kObjectType);
-            if ( !Point32::toJson(poly.points()[i], point, allocator) )
-                return false;
-            points.PushBack(point, allocator);
-        }
-
-        doc.AddMember("points", points, allocator);
-
-        return true;
-    }
-
-    template <class PolygonType, class JsonType>
-    bool _fromJson(const JsonType &doc, PolygonType &poly)
-    {
-        if (!doc.HasMember("points") || !doc["points"].IsArray())
-            return false;
-        const auto &array = doc["points"].GetArray();
-        for (long i=0; i < array.Size(); ++i){
-            if ( !Point32::fromJson(array[i], poly.points()[i]) )
+            if (!value.HasMember("longitude") || !value["longitude"].IsFloat()){
+                assert(false);
                 return false;
             }
-        return true;
-    }
-
-    template<class PolygonType>
-    bool fromJson(const rapidjson::Value &doc,
-                  PolygonType &poly)
-    {
-        return _fromJson(doc, poly);
-    }
-
-    template<class PolygonType>
-    bool fromJson(const rapidjson::Document &doc,
-                  PolygonType &poly)
-    {
-        return _fromJson(doc, poly);
-    }
-
-
-}
-
-namespace PolygonStamped {
-
-
-    //! @brief C++ representation of geometry_msgs/PolygonStamped
-    template <class PointTypeCVR,
-              template<class,class...> class PolygonType = Polygon::Polygon, class HeaderType = Header::Header>
-    class PolygonStamped {
-    typedef typename boost::remove_cv<typename boost::remove_reference<PointTypeCVR>::type>::type PointType;
-        typedef PolygonType<PointType> Polygon;
-    public:
-        PolygonStamped(){};
-        PolygonStamped(const HeaderType &header, Polygon &polygon) :
-            _header(header)
-          , _polygon(polygon)
-        {}
-
-
-        const HeaderType  &header()  const   {return _header;}
-        const Polygon     &polygon() const   {return _polygon;}
-
-        HeaderType  &header()  {return _header;}
-        Polygon     &polygon() {return _polygon;}
-
-        void setHeader(const HeaderType &header)          {_header = header;}
-        void setPolygon(const Polygon &polygon)
-        {
-            _polygon = polygon;
-        }
-
-    private:
-        HeaderType   _header;
-        Polygon      _polygon;
-    };
-
-
-    template <class PolyStamped>
-    bool toJson(const PolyStamped &polyStamped, rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
-    {
-        return toJson(polyStamped.polygon(), polyStamped.header(), doc, allocator);
-    }
-
-    template <class PolygonType, class HeaderType>
-    bool toJson(const PolygonType &poly, const HeaderType &h, rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
-    {
-        rapidjson::Document header(rapidjson::kObjectType);
-        if (!Header::toJson(h, header, allocator))
-            return false;
-        rapidjson::Document polygon(rapidjson::kObjectType);
-        if (!Polygon::toJson(poly, polygon, allocator))
+            if (!value.HasMember("altitude") || !value["altitude"].IsFloat()){
+                assert(false);
                 return false;
-        doc.AddMember("header", header, allocator);
-        doc.AddMember("polygon", polygon, allocator);
-
-        return true;
-    }
-
-    namespace det {
-    template <class PolygonStampedType>
-    bool setHeader(const rapidjson::Value &doc, PolygonStampedType &polyStamped, Int2Type<1>) { // polyStamped.setHeader() exists
-        typedef decltype (polyStamped.header()) HeaderType;
-        HeaderType header;
-        bool ret = Header::fromJson(doc, header);
-        polyStamped.setHeader(header);
-        return ret;
-    }
-    template <class PolygonStampedType>
-    bool setHeader(const rapidjson::Value &doc, PolygonStampedType &polyStamped, Int2Type<0>) { // polyStamped.setHeader() does not exists
-        (void)doc;
-        (void)polyStamped;
-        return true;
-    }
             }
 
-    template <class PolygonType, class HeaderType, class JsonType>
-    bool _fromJson(const JsonType &doc, PolygonType &polyStamped)
-    {
-        if ( !doc.HasMember("header") )
-            return false;
-        if ( !doc.HasMember("polygon") )
-            return false;
-
-
-        typedef traits::HasMemberSetHeader<PolygonType> HasHeader;
-        if ( !det::setHeader(doc["header"], polyStamped, Int2Type<HasHeader::value>()))
-            return false;
-
-        if ( !Polygon::fromJson(doc["polygon"], polyStamped.polygon()) )
-            return false;
+            p.setLatitude(value["latitude"].GetFloat());
+            p.setLongitude(value["longitude"].GetFloat());
+            typedef typename Select<HasMemberSetAltitude<PointType>::value, Has3Components, Has2Components>::Result
+                    Components; // Check if PointType has 2 or 3 dimensions.
+            det::setAltitude(value["altitude"], p,  Type2Type<Components>()); // If T has no member altitude() discard doc["altitude"];
 
             return true;
         }
 
-    template<class PolygonType>
-    bool fromJson(const rapidjson::Value &doc,
-                  PolygonType &poly)
-    {
-        return _fromJson(doc, poly);
-    }
-
-    template<class PolygonType>
-    bool fromJson(const rapidjson::Document &doc,
-                  PolygonType &poly)
-    {
-        return _fromJson(doc, poly);
-    }
+} // GeoPoint
+} // GeographicMsgs
 
-}
 
+//! @brief Namespace containing methodes for jsk_recognition_msgs generation.
+namespace JSKRecognitionMsgs {
+//! @brief Namespace containing methodes for jsk_recognition_msgs/PolygonArray message generation.
 namespace PolygonArray {
 using namespace ROSBridge::traits;
-
-    //! @brief C++ representation of jsk_recognition_msgs/PolygonArray
-    template <class PointTypeCVR,
-              template <class, class...> class Polygon = Polygon::Polygon,
-              template <class, class...> class ContainerType = std::vector,
-              class HeaderType =  Header::Header>
-    class PolygonArray{
-        typedef typename boost::remove_cv<typename boost::remove_reference<PointTypeCVR>::type>::type PointType;
-        typedef Polygon<PointType> PolygonType;
-    public:
-        PolygonArray(){}
-        PolygonArray(const HeaderType                   &header,
-                     const ContainerType<PolygonType>   &polygons,
-                     const ContainerType<uint32_t>      &labels,
-                     const ContainerType<_Float32>      &likelihood)
-          : _header(header)
-          , _polygons(polygons)
-          , _labels(labels)
-          , _likelihood(likelihood)
-        {}
-
-        const HeaderType                 &header()     const {return _header;}
-              HeaderType                 &header()           {return _header;}
-        const ContainerType<PolygonType> &polygons()   const {return _polygons;}
-              ContainerType<PolygonType> &polygons()         {return _polygons;}
-        const ContainerType<uint32_t>    &labels()     const {return _labels;}
-              ContainerType<uint32_t>    &labels()           {return _labels;}
-        const ContainerType<_Float32>    &likelyhood() const {return _likelihood;}
-              ContainerType<_Float32>    &likelyhood()       {return _likelihood;}
-
-        void setHeader     (const HeaderType                &header)        {_header     = &header;}
-        void setPolygons   (ContainerType<PolygonType>      &polygon)       { _polygons   = polygon; }
-        void setLabels     (const ContainerType<uint32_t>    &labels)       {_labels     = labels;}
-        void setLikelihood (const ContainerType<_Float32>    &likelihood)   {_likelihood = likelihood;}
-
-
-
-    private:
-        HeaderType                  _header;
-        ContainerType<PolygonType>  _polygons;
-        ContainerType<uint32_t>     _labels;
-        ContainerType<_Float32>     _likelihood;
-    };
-
+using namespace ROSBridge::JsonMethodes::StdMsgs;
+using namespace ROSBridge::JsonMethodes::GeometryMsgs;
 
     namespace PAdetail {
         //! Helper functions to generate Json entries for labels and likelihood.
@@ -695,33 +474,37 @@ using namespace ROSBridge::traits;
     //! \note If this function is called p.polygons[i] (entries implement the the PolygonStampedGroup interface)
     //! must contain a header.
     template <class PolygonArrayType>
-    bool toJson(const PolygonArrayType &p, rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
+    bool toJson(const PolygonArrayType &pArray, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
     {
         rapidjson::Document header(rapidjson::kObjectType);
-        if (Header::toJson(p.header(), header, allocator))
+        if (Header::toJson(pArray.header(), header, allocator)){
+            assert(false);
             return false;
-        doc.AddMember("header", header, allocator);
+        }
+        value.AddMember("header", header, allocator);
 
         rapidjson::Value polygons(rapidjson::kArrayType);
-        for(unsigned long i=0; i < p.polygons().size(); ++i){
+        for(unsigned long i=0; i < pArray.polygons().size(); ++i){
             rapidjson::Document polygon(rapidjson::kObjectType);
-            if (!PolygonStamped::toJson(p.polygons[i], polygon, allocator))
+            if (!PolygonStamped::toJson(pArray.polygons()[i], polygon, allocator)){
+                assert(false);
                 return false;
+            }
             polygons.PushBack(polygon, allocator);
         }
-        doc.AddMember("polygons", polygons, allocator);
+        value.AddMember("polygons", polygons, allocator);
 
 
         rapidjson::Value labels(rapidjson::kArrayType);
         typedef HasMemberLabels<PolygonArrayType> HasLabels;
-        PAdetail::labelsToJson(p, labels, allocator, Int2Type<HasLabels::value>());
-        doc.AddMember("labels", labels, allocator);
+        PAdetail::labelsToJson(pArray, labels, allocator, Int2Type<HasLabels::value>());
+        value.AddMember("labels", labels, allocator);
 
 
         rapidjson::Value likelihood(rapidjson::kArrayType);
         typedef HasMemberLikelihood<PolygonArrayType> HasLikelihood;
-        PAdetail::likelihoodToJson(p, likelihood, allocator, Int2Type<HasLikelihood::value>());
-        doc.AddMember("likelihood", likelihood, allocator);
+        PAdetail::likelihoodToJson(pArray, likelihood, allocator, Int2Type<HasLikelihood::value>());
+        value.AddMember("likelihood", likelihood, allocator);
 
         return true;
     }
@@ -740,94 +523,138 @@ using namespace ROSBridge::traits;
     //! \note If this function is called the headers in p.polygons[i] (entries implement the the PolygonStampedGroup interface)
     //! are ignored.
     template <class PolygonArrayType, class HeaderType>
-    bool toJson(const PolygonArrayType &p, const HeaderType &h, rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
+    bool toJson(const PolygonArrayType &p, const HeaderType &h, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
     {
         rapidjson::Document header(rapidjson::kObjectType);
-        if (!Header::toJson(h, header, allocator))
+        if (!Header::toJson(h, header, allocator)){
+            assert(false);
             return false;
-        doc.AddMember("header", header, allocator);
+        }
+        value.AddMember("header", header, allocator);
 
         rapidjson::Value polygons(rapidjson::kArrayType);
         for(unsigned long i=0; i < (unsigned long)(p.polygons().size()); ++i){
             rapidjson::Document polygon(rapidjson::kObjectType);
-            if (!PolygonStamped::toJson(p.polygons()[i], h, polygon, allocator))
+            if (!PolygonStamped::toJson(p.polygons()[i].polygon(), h, polygon, allocator)){
+                assert(false);
                 return false;
+            }
             polygons.PushBack(polygon, allocator);
         }
-        doc.AddMember("polygons", polygons, allocator);
+        value.AddMember("polygons", polygons, allocator);
 
 
         rapidjson::Value labels(rapidjson::kArrayType);
         typedef HasMemberLabels<PolygonArrayType> HasLabels;
         PAdetail::labelsToJson(p, labels, allocator, Int2Type<HasLabels::value>());
-        doc.AddMember("labels", labels, allocator);
+        value.AddMember("labels", labels, allocator);
 
 
         rapidjson::Value likelihood(rapidjson::kArrayType);
         typedef HasMemberLikelihood<PolygonArrayType> HasLikelihood;
         PAdetail::likelihoodToJson(p, likelihood, allocator, Int2Type<HasLikelihood::value>());
-        doc.AddMember("likelihood", likelihood, allocator);
+        value.AddMember("likelihood", likelihood, allocator);
 
         return true;
     }
 
-    template <class PolygonArrayType, class JsonType>
-    bool _fromJson(const JsonType &doc, PolygonArrayType &p)
+    template <class PolygonArrayType>
+    bool fromJson(const rapidjson::Value &value, PolygonArrayType &p)
     {
-        if ( !doc.HasMember("header"))
+        if ( !value.HasMember("header")){
+            assert(false);
             return false;
-        if ( !doc.HasMember("polygons") || !doc["polygons"].IsArray() )
+        }
+        if ( !value.HasMember("polygons") || !value["polygons"].IsArray() ){
+            assert(false);
             return false;
-        if ( !doc.HasMember("labels") || !doc["labels"].IsArray() )
+        }
+        if ( !value.HasMember("labels") || !value["labels"].IsArray() ){
+            assert(false);
             return false;
-        if ( !doc.HasMember("likelihood") || !doc["likelihood"].IsArray() )
+        }
+        if ( !value.HasMember("likelihood") || !value["likelihood"].IsArray() ){
+            assert(false);
             return false;
+        }
 
 
-        typedef traits::HasMemberSetHeader<PolygonArrayType> HasHeader;
-        if ( !PolygonStamped::det::setHeader(doc["header"], p, Int2Type<HasHeader::value>()))
+        typedef traits::HasMemberHeader<PolygonArrayType> HasHeader;
+        if ( !PolygonStamped::det::setHeader(value["header"], p, Int2Type<HasHeader::value>())){
+            assert(false);
             return false;
+        }
 
 
-        const auto &polygons = doc["polygons"];
-        p.polygons().reserve(polygons.Size());
-        for (long i=0; i < polygons.Size(); ++i) {
-            if ( !polygons[i].HasMember("header") )
+        const auto &polyStampedJson = value["polygons"];
+        p.polygons().reserve(polyStampedJson.Size());
+        typedef decltype (p.polygons()[0]) PolyStampedCVR;
+        typedef typename boost::remove_cv<typename boost::remove_reference<PolyStampedCVR>::type>::type
+                PolyStamped;
+        for (long i=0; i < polyStampedJson.Size(); ++i) {
+            if ( !polyStampedJson[i].HasMember("header") ){
+                assert(false);
                 return false;
-            typedef traits::HasMemberSetHeader<PolygonArrayType> HasHeader;
-            if ( !PolygonStamped::det::setHeader(polygons[i]["header"], p.polygons[i]().header(), Int2Type<HasHeader::value>()))
+            }
+            PolyStamped polyStamped;
+
+            if ( !PolygonStamped::det::setHeader(polyStampedJson[i]["header"], polyStamped, Int2Type<HasHeader::value>())){
+                assert(false);
                 return false;
+            }
 
-            if ( !Polygon::fromJson(polygons[i], p.polygons[i].points()))
+            if ( !Polygon::fromJson(polyStampedJson[i]["polygon"], polyStamped.polygon())){
+                assert(false);
                 return false;
             }
 
-        typedef traits::HasMemberSetLabels<PolygonArrayType> HasLabels;
-        PAdetail::setLabels(doc["labels"], p, Int2Type<HasLabels::value>());
+            p.polygons().push_back(std::move(polyStamped));
+        }
 
-        typedef traits::HasMemberSetLikelihood<PolygonArrayType> HasLikelihood;
-        PAdetail::setLikelihood(doc["likelihood"], p, Int2Type<HasLikelihood::value>());
+        typedef traits::HasMemberLabels<PolygonArrayType> HasLabels;
+        PAdetail::setLabels(value["labels"], p, Int2Type<HasLabels::value>());
 
+        typedef traits::HasMemberLikelihood<PolygonArrayType> HasLikelihood;
+        PAdetail::setLikelihood(value["likelihood"], p, Int2Type<HasLikelihood::value>());
 
         return true;
     }
 
-    template<class PolygonArrayType>
-    bool fromJson(const rapidjson::Value &doc,
-                  PolygonArrayType &polyArray)
+} // end namespace PolygonArray
+} // namespace JSKRekognitionMsgs
+
+
+//! @brief Namespace containing methodes for nemo_msgs generation.
+namespace NemoMsgs {
+//! @brief Namespace containing methodes for nemo_msgs/Progress generation.
+namespace Progress {
+    template <class ProgressType>
+    bool toJson(const ProgressType &p, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
     {
-        return _fromJson(doc, polyArray);
+        rapidjson::Value progressJson(rapidjson::kArrayType);
+        for(unsigned long i=0; i < p.progress().size(); ++i){
+            progressJson.PushBack(rapidjson::Value().SetInt(std::int8_t(p.progress()[i])), allocator);
+        }
+        value.AddMember("progress", progressJson, allocator);
+
     }
 
-    template<class PolygonArrayType>
-    bool fromJson(const rapidjson::Document &doc,
-                  PolygonArrayType &polyArray)
+    template <class ProgressType>
+    bool fromJson(const rapidjson::Value &value, ProgressType &p)
     {
-        return _fromJson(doc, polyArray);
+        if (!value.HasMember("progress") || !value["progress"].IsArray()){
+            assert(false);
+            return false;
         }
 
-}
-}// end namespace JsonMethodes
-} // end namespace ROSBridge
+        unsigned long sz = value.Size();
+        p.progress().reserve(sz);
+        for (unsigned long i=0; i < sz; ++i)
+            p.progress().push_back(value[i]);
+    }
+} // namespace Progress
+} // namespace NemoMsgs
+} // namespace JsonMethodes
+} // namespace ROSBridge
 
 #endif // MESSAGES_H

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