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