diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index 31a7a020fbe0fff872630e978f8059e77eb8d9da..67fe961d5935543a5b6dcf56408a9d242803ad67 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -433,10 +433,7 @@ HEADERS += \
     src/Wima/Polygon2D.h \
     src/Wima/PolygonArray.h \
     src/Wima/QtROSJsonFactory.h \
-    src/Wima/ROSCommunicator.h \
-    src/Wima/ROSJsonFactory.h \
-    src/Wima/ROSMessageGroups.h \
-    src/Wima/ROSMessageType.h \
+    src/Wima/QtROSTypeFactory.h \
     src/Wima/SnakeTiles.h \
     src/Wima/SnakeTilesLocal.h \
     src/Wima/WimaControllerDetail.h \
@@ -475,8 +472,9 @@ HEADERS += \
     src/Wima/testplanimetrycalculus.h \
     src/Settings/WimaSettings.h \
     src/QmlControls/QmlObjectVectorModel.h \
-    src/comm/ros_bridge/include/ROSMessageTraits.h \
-    src/comm/ros_bridge/include/messages.h \
+    src/comm/ros_bridge/include/JsonMethodes.h \
+    src/comm/ros_bridge/include/MessageTraits.h \
+    src/comm/ros_bridge/include/TypeFactory.h \
     src/comm/utilities.h
 SOURCES += \
     src/Snake/clipper/clipper.cpp \
@@ -484,8 +482,7 @@ SOURCES += \
     src/Snake/snake_geometry.cpp \
     src/Wima/GeoPoint3D.cpp \
     src/Wima/PolygonArray.cc \
-    src/Wima/ROSCommunicator.cpp \
-    src/Wima/ROSJsonFactory.cpp \
+    src/comm/ros_bridge/src/ROSCommunicator.cpp \
     src/Wima/WimaControllerDetail.cc \
     src/Wima/snaketile.cpp \
     src/api/QGCCorePlugin.cc \
diff --git a/src/Wima/.directory b/src/Wima/.directory
new file mode 100644
index 0000000000000000000000000000000000000000..fb64c37e492746510948a33681efb302a4e63cac
--- /dev/null
+++ b/src/Wima/.directory
@@ -0,0 +1,4 @@
+[Dolphin]
+Timestamp=2020,7,8,11,13,28
+Version=4
+ViewMode=1
diff --git a/src/Wima/GeoPoint3D.h b/src/Wima/GeoPoint3D.h
index d9a63aa08f860cbcddb0f298f45a2808dd041612..a45142ecddc88342041cb88b5d4be884a3ab9188 100644
--- a/src/Wima/GeoPoint3D.h
+++ b/src/Wima/GeoPoint3D.h
@@ -1,17 +1,18 @@
 #pragma once
 
-#include "ros_bridge/include/messages.h"
-#include "ROSMessageType.h"
-#include "ROSMessageGroups.h"
+#include "ros_bridge/include/JsonMethodes.h"
+#include "ros_bridge/include/MessageBaseClass.h"
+#include "ros_bridge/include/MessageGroups.h"
 
 #include <QObject>
-typedef ROSMessageType<QString> ROSMsg;
-typedef ros_bridge::GeoPoint::GeoPoint ROSGeoPoint;
+typedef ROSBridge::MessageBaseClass<QString> ROSMsg;
+typedef ROSBridge::JsonMethodes::GeoPoint::GeoPoint ROSGeoPoint;
+namespace MsgGroups = ROSBridge::MessageGroups;
 class GeoPoint3D : public QObject, public ROSMsg, public ROSGeoPoint
 {
     Q_OBJECT
 public:
-    typedef GeoPointGroup Group;
+    typedef MsgGroups::GeoPointGroup Group;
 
     explicit GeoPoint3D(QObject *parent = nullptr)
         : QObject(parent), ROSMsg(), ROSGeoPoint() {}
@@ -25,7 +26,7 @@ public:
                         QObject *parent = nullptr)
         : QObject(parent), ROSMsg(), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
     {}
-    explicit GeoPoint3D(const class ros_bridge::GeoPoint::GeoPoint& p,
+    explicit GeoPoint3D(const ROSGeoPoint& p,
                         QObject *parent = nullptr)
         : QObject(parent), ROSMsg(), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
     {}
diff --git a/src/Wima/Polygon2D.h b/src/Wima/Polygon2D.h
index d6311c90dd929b65d064cf4947267a98e9828fe8..e8567ba863330e5f9e35900986faafdad480faee 100644
--- a/src/Wima/Polygon2D.h
+++ b/src/Wima/Polygon2D.h
@@ -3,14 +3,15 @@
 #include <QPolygonF>
 #include <QPointF>
 
-#include "ROSMessageGroups.h"
-#include "ROSMessageType.h"
+#include "ros_bridge/include/MessageGroups.h"
+#include "ros_bridge/include/MessageBaseClass.h"
 
-typedef ROSMessageType<QString> ROSMsg;
+typedef ROSBridge::MessageBaseClass<QString> ROSMsg;
+namespace MsgGroups = ROSBridge::MessageGroups;
 
 class Polygon2D : public QPolygonF, public ROSMsg{
 public:
-    typedef PolygonGroup Group;
+    typedef MsgGroups::PolygonGroup Group;
 
     Polygon2D(){}
     Polygon2D(const Polygon2D &other) : QPolygonF(other) , ROSMsg(){}
diff --git a/src/Wima/PolygonArray.cc b/src/Wima/PolygonArray.cc
index afc6951c185c0813212ca946fc18eae8ec193785..4dac28602f0b3424bf4bdfbbc3d64ae293284cf6 100644
--- a/src/Wima/PolygonArray.cc
+++ b/src/Wima/PolygonArray.cc
@@ -1,4 +1 @@
 #include "PolygonArray.h"
-
-template <class PolygonType, template <class,class...> class ContainerType >
-const char *PolygonArray<PolygonType, ContainerType>::typeName = "PolygonArray";
diff --git a/src/Wima/PolygonArray.h b/src/Wima/PolygonArray.h
index 17b607ef2355a653ddf168ea6491d7d7725771ad..2cad27b70c6fa13212e0b74b7dc81227bafd2c31 100644
--- a/src/Wima/PolygonArray.h
+++ b/src/Wima/PolygonArray.h
@@ -2,16 +2,14 @@
 
 #include <QObject>
 
-#include "ROSMessageType.h"
-
+#include "ros_bridge/include/MessageBaseClass.h"
 
+typedef ROSBridge::MessageBaseClass<QString> ROSMsgBase;
 template <class PolygonType, template <class,class...> class ContainerType >
-class PolygonArray : public ROSMessageType<QString>, public ContainerType<PolygonType> {
+class PolygonArray : public ROSMsgBase, public ContainerType<PolygonType> {
 public:
-    explicit PolygonArray() : ROSMessageType<QString>(), ContainerType<PolygonType>() {}
+    explicit PolygonArray() : ROSMsgBase(), ContainerType<PolygonType>() {}
     PolygonArray(const PolygonArray &other) : ContainerType<PolygonType>(other) {}
 
-    QString type() const override {return typeName;}
-
-    static const char *typeName;
+    QString type() const override {return "PolygonArray";}
 };
diff --git a/src/Wima/QtROSJsonFactory.h b/src/Wima/QtROSJsonFactory.h
index 36692d2818379c3b6098d1423b0971715ef095b2..450959d6ad455a8363149b46e120545eebe9eca3 100644
--- a/src/Wima/QtROSJsonFactory.h
+++ b/src/Wima/QtROSJsonFactory.h
@@ -1,4 +1,5 @@
 #pragma once
-#include "ROSJsonFactory.h"
+#include "ros_bridge/include/JsonFactory.h"
+#include <QString>
 
-typedef ROSJsonFactory<> QtROSJsonFactory;
+typedef ROSBridge::JsonFactory<QString> QtROSJsonFactory;
diff --git a/src/Wima/QtROSTypeFactory.h b/src/Wima/QtROSTypeFactory.h
new file mode 100644
index 0000000000000000000000000000000000000000..dc2b0ada4e9f4f4c80f0baa7872b99bf3ed68307
--- /dev/null
+++ b/src/Wima/QtROSTypeFactory.h
@@ -0,0 +1,5 @@
+#pragma once
+#include "ros_bridge/include/TypeFactory.h"
+#include <QString>
+
+typedef ROSBridge::TypeFactory<QString> QtROSTypeFactory;
diff --git a/src/Wima/ROSCommunicator.cpp b/src/Wima/ROSCommunicator.cpp
deleted file mode 100644
index 902fed2b055e6826952182e8b669ae82672d3cfe..0000000000000000000000000000000000000000
--- a/src/Wima/ROSCommunicator.cpp
+++ /dev/null
@@ -1,6 +0,0 @@
-#include "ROSCommunicator.h"
-
-void ROSCommunicator::send(const ROSMsg *msg)
-{
-
-}
diff --git a/src/Wima/ROSCommunicator.h b/src/Wima/ROSCommunicator.h
deleted file mode 100644
index 945bce2a7cadcbd3abe0fcbdf9768f313eaf76e1..0000000000000000000000000000000000000000
--- a/src/Wima/ROSCommunicator.h
+++ /dev/null
@@ -1,24 +0,0 @@
-#pragma once
-
-#include "WimaAreaData.h"
-#include "ROSMessageType.h"
-
-typedef ROSMessageType<QString> ROSMsg;
-
-class ROSCommunicator : public QObject
-{
-  Q_OBJECT
-
-public:
-  explicit ROSCommunicator(QObject *parent = 0) :
-      QObject(parent) {}
-
-    void send(const ROSMsg *msg);
-
-
-public slots:
-    virtual void receive(ROSMsg *msg) = 0;
-
-signals:
-  void readAsynkPolledChanged(int value);
-};
diff --git a/src/Wima/ROSJsonFactory.cpp b/src/Wima/ROSJsonFactory.cpp
deleted file mode 100644
index 1e60276df508d718639c71d19617f52e46df0a43..0000000000000000000000000000000000000000
--- a/src/Wima/ROSJsonFactory.cpp
+++ /dev/null
@@ -1,2 +0,0 @@
-#include "ROSJsonFactory.h"
-#include "PolygonArray.h"
diff --git a/src/Wima/ROSMessageGroups.h b/src/Wima/ROSMessageGroups.h
deleted file mode 100644
index 2353b9d4382a06b8423bfa268164fe57da01dd5c..0000000000000000000000000000000000000000
--- a/src/Wima/ROSMessageGroups.h
+++ /dev/null
@@ -1,45 +0,0 @@
-#pragma once
-
-namespace detail {
-}
-
-//!
-//! \note Each calls belonging to a ROS message group must derive from \class ROSMessageType.
-
-//!
-//! \brief The EmptyGroup struct is used by the \class ROSMessageType base class.
-//!
-//! The EmptyGroup struct is used by the \class ROSMessageType base class. Passing a class using this
-//! group will to the \class ROSJsonFactory will lead to a compile-time error.
-struct EmptyGroup        {};
-
-//!
-//! \brief The Point32Group struct is used the mark a class as a ROS Point32 message.
-//!
-//! The Point32Group struct is used the mark a class as a ROS Point32 message.
-struct Point32Group {};
-
-//!
-//! \brief The GeoPointGroup struct is used the mark a class as a ROS geographic_msgs/GeoPoint message.
-//!
-//! The GeoPointGroup struct is used the mark a class as a ROS geographic_msgs/GeoPoint message.
-struct GeoPointGroup {};
-
-//!
-//! \brief The PolygonGroup struct is used the mark a class as a ROS Polygon message.
-//!
-//! The PolygonGroup struct is used the mark a class as a ROS Polygon message.
-struct PolygonGroup {};
-
-//!
-//! \brief The PolygonStampedGroup struct is used the mark a class as a ROS PolygonStamped message.
-//!
-//! The PolygonStampedGroup struct is used the mark a class as a ROS PolygonStamped message.
-struct PolygonStampedGroup {};
-
-
-//!
-//! \brief 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 PolygonArray message.
-struct PolygonArrayGroup {};
diff --git a/src/Wima/ROSMessageType.h b/src/Wima/ROSMessageType.h
deleted file mode 100644
index 6064779abff5dacfd099474b4a622a61db40d82c..0000000000000000000000000000000000000000
--- a/src/Wima/ROSMessageType.h
+++ /dev/null
@@ -1,25 +0,0 @@
-#pragma once
-
-#include "ROSMessageGroups.h"
-
-//! @brief Abstract base class for all ROS Messages Types.
-//!
-//! Abstract base class for all ROS Messages Types. This class defines a basic interface
-//! every class must fullfill if it takes advantages of the \class ROSJsonFactory.
-//! Every class must define the public typedef Group, which associates it to a message Group (\see ROSMessageGroups). The Group type
-//! is used by the \class ROSJsonFactory to determine the type of the message it creates. The
-//! ROSMessageType 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 ROSMessageType{
-public:
-    typedef EmptyGroup Group;
-
-    ROSMessageType() {};
-    ~ROSMessageType() {};
-    // Avoid sliceing (copy with ROSMessage::Clone)!
-    ROSMessageType(const ROSMessageType &) = delete;
-    ROSMessageType& operator=(const ROSMessageType &) = delete;
-
-    virtual ROSMessageType* Clone() const = 0;
-};
diff --git a/src/Wima/SnakeTiles.h b/src/Wima/SnakeTiles.h
index fdfd4954f7949aff1188dd1233e32b909c7a1d01..dcb146af5eb9225114a84d78fb251a8e3f601a02 100644
--- a/src/Wima/SnakeTiles.h
+++ b/src/Wima/SnakeTiles.h
@@ -1,4 +1,5 @@
-#ifndef SNAKETILES_H
-#define SNAKETILES_H
+#pragma once
+#include "snaketile.h"
+#include "WimaPolygonArray.h"
 
-#endif // SNAKETILES_H
+typedef WimaPolygonArray<SnakeTile, QVector> SnakeTiles;
diff --git a/src/Wima/SnakeTilesLocal.h b/src/Wima/SnakeTilesLocal.h
index 03630c20cbd39fd14a95a5de2ea8d99fafde6a4d..a396de7d7883eb97b78783c6cb70227c195844b7 100644
--- a/src/Wima/SnakeTilesLocal.h
+++ b/src/Wima/SnakeTilesLocal.h
@@ -1,4 +1,8 @@
-#ifndef SNAKETILELOCAL_H
-#define SNAKETILELOCAL_H
+#pragma once
 
-#endif // SNAKETILELOCAL_H
+#include "ros_bridge/include/MessageGroups.h"
+#include "Polygon2D.h"
+#include "WimaPolygonArray.h"
+
+namespace  MsgGroups = ROSBridge::MessageGroups;
+typedef WimaPolygonArray<Polygon2D, QVector, MsgGroups::PolygonArrayGroup> SnakeTilesLocal;
diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc
index ec62373cbcd59c0c3ee3f8eb72ee65915e359f93..d6f7eddbfd51fd96607a9c84574edc125edfd275 100644
--- a/src/Wima/WimaController.cc
+++ b/src/Wima/WimaController.cc
@@ -1,11 +1,12 @@
 #include "WimaController.h"
 #include "utilities.h"
-#include "ros_bridge/include/messages.h"
+#include "ros_bridge/include/JsonMethodes.h"
 #include "ros_bridge/rapidjson/include/rapidjson/document.h"
 #include "ros_bridge/rapidjson/include/rapidjson/writer.h"
 #include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h"
 
 #include "QtROSJsonFactory.h"
+#include "QtROSTypeFactory.h"
 
 #include "time.h"
 #include "assert.h"
@@ -613,13 +614,14 @@ bool WimaController::_fetchContainerData()
             _snakeTilesLocal.polygons().append(Tile);
         }
 
-        QtROSJsonFactory factory;
-        QScopedPointer<rapidjson::Document> doc(factory.create(_snakeTilesLocal));
+        QtROSJsonFactory JsonFactory;
+        QScopedPointer<rapidjson::Document> doc(JsonFactory.create(_snakeTilesLocal));
 
         auto &temp = _scenario.getOrigin();
         ::GeoPoint3D origin(temp[0], temp[1], temp[2]);
-        QScopedPointer<rapidjson::Document> doc2(factory.create(origin));
+        QScopedPointer<rapidjson::Document> doc2(JsonFactory.create(origin));
 
+        cout.precision(10);
         cout << "Origin" << endl;
         rapidjson::OStreamWrapper out(std::cout);
         rapidjson::Writer<rapidjson::OStreamWrapper> writer(out);
@@ -628,9 +630,25 @@ bool WimaController::_fetchContainerData()
 
         cout << "Snake Tiles" << endl;
         rapidjson::Writer<rapidjson::OStreamWrapper> writer2(out);
-        doc->Accept(writer2);
+        //doc->Accept(writer2);
         std::cout << std::endl << std::endl;
 
+        QtROSTypeFactory TypeFactory;
+      ::GeoPoint3D origin_same;
+        TypeFactory.create(*doc2.data(), origin_same);
+
+        cout << "Origin2" << endl;
+        std::cout << origin_same << std::endl;
+
+        std::cout << "TypeFactory test true: ";
+        bool isSame = origin_same == origin;
+        std::cout << isSame << endl;
+
+
+        ::SnakeTilesLocal tiles_same;
+          TypeFactory.create(*doc.data(), tiles_same);
+
+
 
     }
 
diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h
index abb204a06591d775a5c3f4af924967a8d5dbd8e9..3804c84fff6e694383aa3574ad26015e89bd7b4e 100644
--- a/src/Wima/WimaController.h
+++ b/src/Wima/WimaController.h
@@ -26,10 +26,10 @@
 
 #include "snake.h"
 #include "WimaControllerDetail.h"
-#include "WimaPolygonArray.h"
-#include "snaketile.h"
-#include "ROSMessageGroups.h"
-#include "Polygon2D.h"
+//#include "snaketile.h"
+//#include "Polygon2D.h"
+#include "SnakeTiles.h"
+#include "SnakeTilesLocal.h"
 #include "GeoPoint3D.h"
 
 #define CHECK_BATTERY_INTERVAL 1000 // ms
@@ -327,11 +327,7 @@ private:
     SettingsFact                 _snakeMinTileArea;
     SettingsFact                 _snakeLineDistance;
     SettingsFact                 _snakeMinTransectLength;
-
-    typedef WimaPolygonArray<SnakeTile, QVector> SnakeTiles;
-    SnakeTiles                    _snakeTiles; // tiles
-
-    typedef WimaPolygonArray<Polygon2D, QVector, PolygonArrayGroup> SnakeTilesLocal;
+    SnakeTiles                   _snakeTiles; // tiles
     SnakeTilesLocal              _snakeTilesLocal; // tiles local coordinate system
     QVariantList                 _snakeTileCenterPoints;
     QList<int>                   _snakeProgress; // measurement progress
diff --git a/src/Wima/WimaPolygonArray.h b/src/Wima/WimaPolygonArray.h
index 535d2afca67f5a0d6a392258f12274e32a00ebac..1782e9103805298ceaab54c9edef359f9a305ffe 100644
--- a/src/Wima/WimaPolygonArray.h
+++ b/src/Wima/WimaPolygonArray.h
@@ -1,13 +1,15 @@
 #pragma once
-#include "ROSMessageType.h"
-#include "ROSMessageGroups.h"
+#include "ros_bridge/include/MessageBaseClass.h"
+#include "ros_bridge/include/MessageGroups.h"
 
 #include "QmlObjectListModel.h"
 
 #include <QVector>
 #include <QString>
 
-typedef ROSMessageType<QString> ROSMsg;
+typedef ROSBridge::MessageBaseClass<QString> ROSMsg;
+namespace MsgGroups = ROSBridge::MessageGroups;
+typedef MsgGroups::EmptyGroup EmptyGroup;
 template <class PolygonType, template <class, class...> class ContainerType = QVector, typename GroupType = EmptyGroup>
 class WimaPolygonArray : public ROSMsg
 {
@@ -19,7 +21,7 @@ public:
       , _polygons(other._polygons), _dirty(true)
     {}
 
-    virtual ROSMessageType<QString> * Clone() const override{
+    virtual MessageBaseClass<QString> * Clone() const override{
         return new WimaPolygonArray(*this);
     }
 
diff --git a/src/Wima/ROSJsonFactory.h b/src/comm/ros_bridge/include/JsonFactory.h
similarity index 61%
rename from src/Wima/ROSJsonFactory.h
rename to src/comm/ros_bridge/include/JsonFactory.h
index 81516367f72e5fadb84603bfdccc89e0358100b7..ebad92bc12ed678f749623e139269ae3e5ba42cd 100644
--- a/src/Wima/ROSJsonFactory.h
+++ b/src/comm/ros_bridge/include/JsonFactory.h
@@ -1,49 +1,48 @@
-#ifndef ROSJSONFACTORY_H
-#define ROSJSONFACTORY_H
+#pragma once
 
 #include "ros_bridge/rapidjson/include/rapidjson/document.h"
-#include "ros_bridge/include/messages.h"
-#include "ROSMessageType.h"
-#include <QString>
-#include "WimaAreaData.h"
+#include "ros_bridge/include/JsonMethodes.h"
+#include "MessageBaseClass.h"
 #include "utilities.h"
-#include "ros_bridge/include/ROSMessageTraits.h"
+#include "ros_bridge/include/MessageTraits.h"
 
-#include "ROSMessageGroups.h"
+#include "ros_bridge/include/MessageGroups.h"
 
 #include "boost/type_traits.hpp"
 #include "boost/type_traits/is_base_of.hpp"
 
+namespace ROSBridge {
+
 class StdHeaderPolicy;
 
 //!
-//! \brief The ROSJsonFactory class is used to create ROS messages.
+//! \brief The JsonFactory class is used to create ROS messages.
 //!
-//! The ROSJsonFactory class is used to create \class rapidjson::Document documents containing ROS messages
-//! from classes derived from \class ROSMessageType. Each class has a group mark (typedef ... Group) which allows the
-//! ROSJsonFactory to determine the ROS message type it will create.
-template <class HeaderPolicy = StdHeaderPolicy>
-class ROSJsonFactory : public HeaderPolicy
+//! 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>
+class JsonFactory : public HeaderPolicy
 {
-    typedef QString StringType;
-    typedef ROSMessageType<StringType> ROSMsg;
+    typedef MessageBaseClass<StringType> ROSMsg;
 public:
 
-    ROSJsonFactory(){}
+    JsonFactory(){}
 
     //!
     //! \brief Creates a \class rapidjson::Document document containing a ROS mesage from \p msg.
     //!
     //! Creates a \class rapidjson::Document document containing a ROS message from \p msg.
     //! A compile-time error will occur, if \p msg belongs to the \struct EmptyGroup or is
-    //! not derived from \class ROSMessageType.
-    //! \param msg Instance of a \class ROSMessageType subclass belonging to a ROSMessageGroup.
+    //! not derived from \class MessageBaseClass.
+    //! \param msg Instance of a \class MessageBaseClass subclass belonging to a ROSMessageGroup.
     //! \return rapidjson::Document document containing a ROS message.
     template <class T>
     rapidjson::Document *create(const T &msg){
-        static_assert(boost::is_base_of<ROSMsg, T>::value, "Type of msg must be derived from ROSMessageType.");
-        static_assert(!::boost::is_same<typename T::Group, EmptyGroup>::value,
+        static_assert(boost::is_base_of<ROSMsg, T>::value, "Type of msg must be derived from MessageBaseClass.");
+        static_assert(!::boost::is_same<typename T::Group, MessageGroups::EmptyGroup>::value,
                 "msg belongs to group EmptyGroup (not allowed). Please specify Group (typedef MessageGroup Group)");
+        //cout << T::Group::label() << endl;
         return _create(msg, Type2Type<typename T::Group>());
     }
 
@@ -62,10 +61,10 @@ private:
     // Point32Group
     // ===========================================================================
     template<class U>
-    rapidjson::Document *_create(const U &msg, Type2Type<Point32Group>){
-        using namespace ros_bridge;
+    rapidjson::Document *_create(const U &msg, Type2Type<MessageGroups::Point32Group>){
+        using namespace ROSBridge;
         rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
-        bool ret = Point32::toJson<_Float32>(msg, *doc, doc->GetAllocator());
+        bool ret = JsonMethodes::Point32::toJson<_Float32>(msg, *doc, doc->GetAllocator());
         assert(ret);
         (void)ret;
 
@@ -76,10 +75,10 @@ private:
     // GeoPointGroup
     // ===========================================================================
     template<class U>
-    rapidjson::Document *_create(const U &msg, Type2Type<GeoPointGroup>){
-        using namespace ros_bridge;
+    rapidjson::Document *_create(const U &msg, Type2Type<MessageGroups::GeoPointGroup>){
+        using namespace ROSBridge;
         rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
-        bool ret = GeoPoint::toJson(msg, *doc, doc->GetAllocator());
+        bool ret = JsonMethodes::GeoPoint::toJson(msg, *doc, doc->GetAllocator());
         assert(ret);
         (void)ret;
 
@@ -90,10 +89,10 @@ private:
     // PolygonGroup
     // ===========================================================================
     template<class U>
-    rapidjson::Document *_create(const U &msg, Type2Type<PolygonGroup>){
-        using namespace ros_bridge;
+    rapidjson::Document *_create(const U &msg, Type2Type<MessageGroups::PolygonGroup>){
+        using namespace ROSBridge;
         rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
-        bool ret = Polygon::toJson(msg, *doc, doc->GetAllocator());
+        bool ret = JsonMethodes::Polygon::toJson(msg, *doc, doc->GetAllocator());
         assert(ret);
         (void)ret;
 
@@ -104,9 +103,9 @@ private:
     // PolygonStampedGroup
     // ===========================================================================
     template<class U>
-    rapidjson::Document *_create(const U &msg, Type2Type<PolygonStampedGroup>){
-        using namespace ros_bridge;
-        using namespace ros_bridge::traits;
+    rapidjson::Document *_create(const U &msg, Type2Type<MessageGroups::PolygonStampedGroup>){
+        using namespace ROSBridge;
+        using namespace ROSBridge::traits;
         typedef HasMemberHeader<U> HasHeader;
 
         return _createPolygonStamped(msg, Int2Type<HasHeader::value>());
@@ -114,9 +113,9 @@ private:
 
     template<class U, int k>
     rapidjson::Document *_createPolygonStamped(const U &msg, Int2Type<k>){ // U has member header(), use integraded header.
-        using namespace ros_bridge;
+        using namespace ROSBridge;
         rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
-        bool ret = PolygonStamped::toJson(msg, *doc, doc->GetAllocator());
+        bool ret = JsonMethodes::PolygonStamped::toJson(msg, *doc, doc->GetAllocator());
         assert(ret);
         (void)ret;
 
@@ -126,10 +125,10 @@ private:
 
     template<class U>
     rapidjson::Document *_createPolygonStamped(const U &msg, Int2Type<0>){// U has no member header(), generate one on the fly.
-        using namespace ros_bridge;
-        Header::Header header(HeaderPolicy::header(msg));
+        using namespace ROSBridge;
+        JsonMethodes::Header::Header header(HeaderPolicy::header(msg));
         rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
-        bool ret = PolygonStamped::toJson(msg, header, *doc, doc->GetAllocator());
+        bool ret = JsonMethodes::PolygonStamped::toJson(msg, header, *doc, doc->GetAllocator());
         assert(ret);
         (void)ret;
 
@@ -140,9 +139,9 @@ private:
     // PolygonArrayGroup
     // ===========================================================================
     template<class U>
-    rapidjson::Document *_create(const U &msg, Type2Type<PolygonArrayGroup>){
-        using namespace ros_bridge;
-        using namespace ros_bridge::traits;
+    rapidjson::Document *_create(const U &msg, Type2Type<MessageGroups::PolygonArrayGroup>){
+        using namespace ROSBridge;
+        using namespace ROSBridge::traits;
         typedef HasMemberHeader<U> HasHeader;
 
         return _createPolygonArray(msg, Int2Type<HasHeader::value>());
@@ -150,9 +149,9 @@ private:
 
     template<class U, int k>
     rapidjson::Document *_createPolygonArray(const U &msg, Int2Type<k>){ // U has member header(), use integraded header.
-        using namespace ros_bridge;
+        using namespace ROSBridge;
         rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
-        bool ret = PolygonArray::toJson(msg, *doc, doc->GetAllocator());
+        bool ret = JsonMethodes::PolygonArray::toJson(msg, *doc, doc->GetAllocator());
         assert(ret);
         (void)ret;
 
@@ -162,10 +161,10 @@ private:
 
     template<class U>
     rapidjson::Document *_createPolygonArray(const U &msg, Int2Type<0>){// U has no member header(), generate one on the fly.
-        using namespace ros_bridge;
-        Header::Header header(HeaderPolicy::header(msg));
+        using namespace ROSBridge;
+        JsonMethodes::Header::Header header(HeaderPolicy::header(msg));
         rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
-        bool ret = PolygonArray::toJson(msg, header, *doc, doc->GetAllocator());
+        bool ret = JsonMethodes::PolygonArray::toJson(msg, header, *doc, doc->GetAllocator());
         assert(ret);
         (void)ret;
 
@@ -184,8 +183,8 @@ public:
     //! \return Returns the header belonging to msg.
     //!
     template<typename T>
-        ros_bridge::Header::Header header(const T&msg) {
-        return ros_bridge::Header::Header(++_seq, time(msg), "/map");
+        ROSBridge::JsonMethodes::Header::Header header(const T&msg) {
+        return ROSBridge::JsonMethodes::Header::Header(++_seq, time(msg), "/map");
     }
 
 
@@ -193,12 +192,14 @@ public:
     //! \brief time Returns the current time.
     //! \return Returns the current time.
     template<typename T>
-    ros_bridge::Time::Time time(const T&msg) {
+    ROSBridge::JsonMethodes::Time::Time time(const T&msg) {
         (void)msg;
-        return ros_bridge::Time::Time(0,0);
+        return ROSBridge::JsonMethodes::Time::Time(0,0);
     }
 private:
     long _seq;
 };
 
-#endif // ROSJSONFACTORY_H
+} // end namespace ros_bridge
+
+
diff --git a/src/comm/ros_bridge/include/messages.h b/src/comm/ros_bridge/include/JsonMethodes.h
similarity index 57%
rename from src/comm/ros_bridge/include/messages.h
rename to src/comm/ros_bridge/include/JsonMethodes.h
index 472fe11ff2942dcd098a7227febfa1b50b5b1acd..eaa36f815040ee297981c12b49762a8e80d4341d 100644
--- a/src/comm/ros_bridge/include/messages.h
+++ b/src/comm/ros_bridge/include/JsonMethodes.h
@@ -10,9 +10,13 @@
 #include "ros_bridge/rapidjson/include/rapidjson/document.h"
 
 #include "utilities.h"
-#include "ROSMessageTraits.h"
+#include "MessageTraits.h"
+#include <ostream>
 
-namespace ros_bridge {
+namespace ROSBridge {
+namespace JsonMethodes {
+
+typedef std::ostream OStream;
 
 namespace Time {
     //! @brief C++ representation of std_msgs/Time
@@ -43,6 +47,34 @@ namespace Time {
         return true;
     }
 
+    template<class TimeType, class JsonType>
+    bool _fromJson(const JsonType &doc,
+                  TimeType &time)
+    {
+        if (!doc.HasMember("secs") || !doc["secs"].IsUint())
+            return false;
+        if (!doc.HasMember("nsecs")|| !doc["nsecs"].IsUint())
+            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);
+    }
+
+    template<class TimeType>
+    bool fromJson(const rapidjson::Value &doc,
+                  TimeType &time)
+    {
+        return _fromJson(doc, time);
+    }
+
 }
 
 namespace Header {
@@ -95,10 +127,44 @@ namespace Header {
         return true;
     }
 
+    template <class HeaderType, class JsonType>
+    bool _fromJson(const JsonType &doc,
+                  HeaderType &header)     {
+        if (!doc.HasMember("seq")|| !doc["seq"].IsUint())
+            return false;
+        if (!doc.HasMember("stamp"))
+            return false;
+        if (!doc.HasMember("frame_id")|| !doc["frame_id"].IsString())
+            return false;
+        header.setSeq(doc["seq"].GetUint());
+        decltype(header.stamp()) time;
+        if (!Time::fromJson(doc["stamp"], time))
+            return false;
+        header.setStamp(time);
+        header.setFrameId(doc["frame_id"].GetString());
+
+        return true;
+    }
+
+
+    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);
+    }
+
 }
 
 namespace Point32 {
-using namespace ros_bridge::traits;
+using namespace ROSBridge::traits;
 
         //! @brief C++ representation of geometry_msgs/Point32.
         template<typename FloatType = _Float64>
@@ -126,39 +192,88 @@ using namespace ros_bridge::traits;
 
         namespace det { //detail
 
-            template <typename FloatType, class T, typename V>
-            bool toJson(const T &p, rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator, Type2Type<V>)
+            template <class T>
+            auto getZ(const T &p, Type2Type<Has3Components>)
+            {
+                return p.z();
+            }
+
+            template <class T>
+            auto getZ(const T &p, Type2Type<Has2Components>)
             {
-                doc.AddMember("x", rapidjson::Value().SetFloat((FloatType)p.x()), allocator);
-                doc.AddMember("y", rapidjson::Value().SetFloat((FloatType)p.y()), allocator);
-                doc.AddMember("z", rapidjson::Value().SetFloat((FloatType)p.z()), allocator);
+                (void)p;
+                return 0.0; // p has no member z() -> add 0.
+            }
 
+            template <class T, typename V>
+            bool setZ(const rapidjson::Value &doc, const T &p, Type2Type<V>)
+            {
+                p.setZ(doc["z"].GetFloat());
                 return true;
             }
 
-            template <typename FloatType, class T>
-            bool toJson(const T &p, rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator, Type2Type<Has2Components>)
+            template <class T>
+            bool setZ(const rapidjson::Value &doc, const T &p, Type2Type<Has2Components>)
             {
-                doc.AddMember("x", rapidjson::Value().SetFloat((FloatType)p.x()), allocator);
-                doc.AddMember("y", rapidjson::Value().SetFloat((FloatType)p.y()), allocator);
-                doc.AddMember("z", rapidjson::Value().SetFloat((FloatType)0.0), allocator); // _point has no member z() -> add 0.
-
+                (void)doc;
+                (void)p;
                 return true;
             }
         }
 
-        template <typename FloatType = _Float32, class T>
+        template <class T>
         bool toJson(const T&p, rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
         {
+            doc.AddMember("x", rapidjson::Value().SetFloat(p.x()), allocator);
+            doc.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.
-            return det::toJson<FloatType>(p, doc, allocator,  Type2Type<Components>());
+            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);
+
+            return true;
+        }
+
+        template <class PointType, class JsonType>
+        bool _fromJson(const JsonType &doc,
+                      PointType &p)     {
+            if (!doc.HasMember("x") || !doc["x"].IsFloat())
+                return false;
+            if (!doc.HasMember("y") || !doc["y"].IsFloat())
+                return false;
+            if (!doc.HasMember("z") || !doc["z"].IsFloat())
+                return false;
+
+            p.setX(doc["x"].GetFloat());
+            p.setY(doc["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"].
+
+            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 GeoPoint {
-using namespace ros_bridge::traits;
+using namespace ROSBridge::traits;
 
         //! @brief C++ representation of geographic_msgs/GeoPoint.
         class GeoPoint{
@@ -183,6 +298,23 @@ using namespace ros_bridge::traits;
             void setAltitude    (_Float64 altitude)    {_altitude  = altitude;}
 
 
+
+            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 std::ostream& operator<<(std::ostream& os, const GeoPoint& p)
+            {
+                os << "lat: " << p._latitude << " lon: " << p._longitude<< " alt: " << p._altitude << "\n";
+                return os;
+            }
+
         private:
             _Float64 _latitude;
             _Float64 _longitude;
@@ -191,33 +323,80 @@ using namespace ros_bridge::traits;
 
         namespace det { //detail
 
-            template <class T, typename V>
-            bool toJson(const T &p, rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator, Type2Type<V>)
+            template <class T>
+            auto getAltitude(const T &p, Type2Type<Has3Components>)
             {
-                doc.AddMember("latitude", rapidjson::Value().SetFloat((_Float64)p.latitude()), allocator);
-                doc.AddMember("longitude", rapidjson::Value().SetFloat((_Float64)p.longitude()), allocator);
-                doc.AddMember("altitude", rapidjson::Value().SetFloat((_Float64)p.altitude()), allocator);
+                return p.altitude();
+            }
 
-                return true;
+            template <class T>
+            auto getAltitude(const T &p, Type2Type<Has2Components>)
+            {
+                (void)p;
+                return 0.0;
             }
 
             template <class T>
-            bool toJson(const T &p, rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator, Type2Type<Has2Components>)
+            void setAltitude(const rapidjson::Value &doc, T &p, Type2Type<Has3Components>)
             {
-                doc.AddMember("latitudex", rapidjson::Value().SetFloat((_Float64)p.latitude()), allocator);
-                doc.AddMember("longitude", rapidjson::Value().SetFloat((_Float64)p.longitude()), allocator);
-                doc.AddMember("altitude", rapidjson::Value().SetFloat((_Float64)0.0), allocator); // _point has no member altitude() -> add 0.
+                p.setAltitude(doc.GetFloat());
+            }
 
-                return true;
+            template <class T>
+            void setAltitude(const rapidjson::Value &doc, T &p, Type2Type<Has2Components>)
+            {
+                (void)doc;
+                (void)p;
             }
         }
 
         template <class T>
         bool toJson(const T&p, rapidjson::Document &doc, rapidjson::Document::AllocatorType &allocator)
         {
+            doc.AddMember("latitude", rapidjson::Value().SetFloat((_Float64)p.latitude()), allocator);
+            doc.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.
-            return det::toJson(p, doc, allocator,  Type2Type<Components>());
+            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);
+            return true;
+        }
+
+
+
+        template <class PointType, class JsonType>
+        bool _fromJson(const JsonType &doc,
+                      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())
+                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);
         }
 
 }
@@ -248,7 +427,7 @@ namespace Polygon {
 
         for(unsigned long i=0; i < (unsigned long)poly.points().size(); ++i) {
             rapidjson::Document point(rapidjson::kObjectType);
-            if ( !Point32::toJson<_Float32>(poly.points()[i], point, allocator) )
+            if ( !Point32::toJson(poly.points()[i], point, allocator) )
                 return false;
             points.PushBack(point, allocator);
         }
@@ -258,6 +437,33 @@ namespace Polygon {
         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]) )
+                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);
+    }
+
 
 }
 
@@ -317,10 +523,60 @@ namespace PolygonStamped {
         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;
+
+        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 PolygonArray {
-using namespace ros_bridge::traits;
+using namespace ROSBridge::traits;
 
     //! @brief C++ representation of jsk_recognition_msgs/PolygonArray
     template <class PointTypeCVR,
@@ -332,13 +588,6 @@ using namespace ros_bridge::traits;
         typedef Polygon<PointType> PolygonType;
     public:
         PolygonArray(){}
-
-        //!
-        //! \brief PolygonArray
-        //! \param header Header
-        //! \param polygons Polygons
-        //! \param labels Labels
-        //! \param likelihood Header
         PolygonArray(const HeaderType                   &header,
                      const ContainerType<PolygonType>   &polygons,
                      const ContainerType<uint32_t>      &labels,
@@ -403,6 +652,34 @@ using namespace ros_bridge::traits;
             for(unsigned long i=0; i < (unsigned long)p.polygons().size(); ++i)
                 likelyhood.PushBack(rapidjson::Value().SetFloat(0), allocator); // use zero!
         }
+
+        //! \note \p p has member \fn labels().
+        template <class PolygonArrayType, int k>
+        void setLabels(const rapidjson::Value &doc, PolygonArrayType &p, Int2Type<k>){
+            for(unsigned long i=0; i < (unsigned long)doc.Size(); ++i)
+                p.labels().push_back(doc[i]);
+        }
+
+        //! \note \p p has no member \fn labels().
+        template <class PolygonArrayType>
+        void setLabels(const rapidjson::Value &doc, PolygonArrayType &p, Int2Type<0>){
+            (void)doc;
+            (void)p;
+        }
+
+        //! \note \p p has member \fn likelihood().
+        template <class PolygonArrayType, int k>
+        void setLikelihood(const rapidjson::Value &doc, PolygonArrayType &p, Int2Type<k>){
+            for(unsigned long i=0; i < (unsigned long)doc.Size(); ++i)
+                p.likelihood().push_back(doc[i]);
+        }
+
+        //! \note \p p has no member \fn likelihood().
+        template <class PolygonArrayType>
+        void setLikelihood(const rapidjson::Value &doc, PolygonArrayType &p, Int2Type<0>){
+            (void)doc;
+            (void)p;
+        }
     }
 
 
@@ -494,7 +771,63 @@ using namespace ros_bridge::traits;
         return true;
     }
 
+    template <class PolygonArrayType, class JsonType>
+    bool _fromJson(const JsonType &doc, PolygonArrayType &p)
+    {
+        if ( !doc.HasMember("header"))
+            return false;
+        if ( !doc.HasMember("polygons") || !doc["polygons"].IsArray() )
+            return false;
+        if ( !doc.HasMember("labels") || !doc["labels"].IsArray() )
+            return false;
+        if ( !doc.HasMember("likelihood") || !doc["likelihood"].IsArray() )
+            return false;
+
+
+        typedef traits::HasMemberSetHeader<PolygonArrayType> HasHeader;
+        if ( !PolygonStamped::det::setHeader(doc["header"], p, Int2Type<HasHeader::value>()))
+            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") )
+                return false;
+            typedef traits::HasMemberSetHeader<PolygonArrayType> HasHeader;
+            if ( !PolygonStamped::det::setHeader(polygons[i]["header"], p.polygons[i]().header(), Int2Type<HasHeader::value>()))
+                return false;
+
+            if ( !Polygon::fromJson(polygons[i], p.polygons[i].points()))
+                return false;
+        }
+
+        typedef traits::HasMemberSetLabels<PolygonArrayType> HasLabels;
+        PAdetail::setLabels(doc["labels"], p, Int2Type<HasLabels::value>());
+
+        typedef traits::HasMemberSetLikelihood<PolygonArrayType> HasLikelihood;
+        PAdetail::setLikelihood(doc["likelihood"], p, Int2Type<HasLikelihood::value>());
+
+
+        return true;
+    }
+
+    template<class PolygonArrayType>
+    bool fromJson(const rapidjson::Value &doc,
+                  PolygonArrayType &polyArray)
+    {
+        return _fromJson(doc, polyArray);
+    }
+
+    template<class PolygonArrayType>
+    bool fromJson(const rapidjson::Document &doc,
+                  PolygonArrayType &polyArray)
+    {
+        return _fromJson(doc, polyArray);
+    }
+
 }
-}
+}// end namespace JsonMethodes
+} // end namespace ROSBridge
 
 #endif // MESSAGES_H
diff --git a/src/comm/ros_bridge/include/MessageBaseClass.h b/src/comm/ros_bridge/include/MessageBaseClass.h
new file mode 100644
index 0000000000000000000000000000000000000000..d7e9baefc0963811cb39beae2ef65a82df8fa475
--- /dev/null
+++ b/src/comm/ros_bridge/include/MessageBaseClass.h
@@ -0,0 +1,29 @@
+#pragma once
+
+#include "MessageGroups.h"
+
+namespace ROSBridge {
+
+//! @brief Abstract base class for all ROS Messages Types.
+//!
+//! Abstract base class for all ROS Messages Types. This class defines a basic interface
+//! every class must fullfill if it takes advantages of the \class ROSJsonFactory.
+//! Every class must define the public typedef Group, which associates it to a message Group (\see MessageGroups). The Group type
+//! 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;
+
+    MessageBaseClass() {};
+    ~MessageBaseClass() {};
+    // Avoid sliceing (copy with ROSMessage::Clone or define subclass operator=)!
+    MessageBaseClass(const MessageBaseClass &) = delete;
+    MessageBaseClass& operator=(const MessageBaseClass &) = delete;
+
+    virtual MessageBaseClass* Clone() const = 0;
+};
+
+}
diff --git a/src/comm/ros_bridge/include/MessageGroups.h b/src/comm/ros_bridge/include/MessageGroups.h
new file mode 100644
index 0000000000000000000000000000000000000000..31bb4625baf379d9b1fac4f29f8d73a2bad26bf1
--- /dev/null
+++ b/src/comm/ros_bridge/include/MessageGroups.h
@@ -0,0 +1,114 @@
+#pragma once
+
+#include <string>
+
+namespace ROSBridge {
+
+namespace MessageGroups {
+
+typedef std::string StringType;
+
+template<typename Group, typename SubGroup, typename...MoreSubGroups>
+struct MessageGroup {
+    static StringType label() {return _label<Group, SubGroup, MoreSubGroups...>();}
+
+    template<typename G, typename SubG, typename...MoreSubG>
+    static StringType _label() {return G::label()+ "/" + _label<SubG, MoreSubG...>(); }
+    template<typename G>
+    static StringType _label() {return G::label(); }
+};
+
+//!
+//! \note Each class belonging to a ROS message group must derive from \class MessageBaseClass.
+
+namespace detail {
+
+//!
+//! \brief The EmptyGroup struct is used by the \class MessageBaseClass base class.
+//!
+//! The EmptyGroup struct is used by the \class MessageBaseClass base class. Passing a class using this
+//! group will to the \class JsonFactory will lead to a compile-time error.
+struct EmptyGroup        {
+    static StringType label() {return "";}
+};
+}
+
+
+struct GeometryMsgs {
+
+    static StringType label() {return "geometry_msgs";}
+    //!
+    //! \brief The Point32Group struct is used the mark a class as a ROS Point32 message.
+    //!
+    //! The Point32Group struct is used the mark a class as a ROS Point32 message.
+    struct Point32Group {
+        static StringType label() {return "Point32";}
+    };
+
+    //!
+    //! \brief The GeoPointGroup struct is used the mark a class as a ROS geographic_msgs/GeoPoint message.
+    //!
+    //! The GeoPointGroup struct is used the mark a class as a ROS geographic_msgs/GeoPoint message.
+    struct GeoPointGroup {
+        static StringType label() {return "GeoPoint";}
+    };
+
+
+    //!
+    //! \brief The PolygonGroup struct is used the mark a class as a ROS Polygon message.
+    //!
+    //! The PolygonGroup struct is used the mark a class as a ROS Polygon message.
+    struct PolygonGroup {
+        static StringType label() {return "Polygon";}
+    };
+
+    //!
+    //! \brief The PolygonStampedGroup struct is used the mark a class as a ROS PolygonStamped message.
+    //!
+    //! The PolygonStampedGroup struct is used the mark a class as a ROS PolygonStamped message.
+    struct PolygonStampedGroup {
+        static StringType label() {return "PolygonStamped";}
+    };
+};
+
+struct JSKRecognitionMsgs {
+
+    static StringType label() {return "jsk_recognition_msgs";}
+
+    //!
+    //! \brief 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 PolygonArray message.
+    struct PolygonArrayGroup {
+        static StringType label() {return "PolygonArray";}
+    };
+};
+
+
+typedef MessageGroup<detail::EmptyGroup,
+                     detail::EmptyGroup>
+        EmptyGroup;
+
+typedef MessageGroups::MessageGroup<MessageGroups::GeometryMsgs,
+                                    MessageGroups::GeometryMsgs::Point32Group>
+        Point32Group;
+
+typedef MessageGroups::MessageGroup<MessageGroups::GeometryMsgs,
+                                    MessageGroups::GeometryMsgs::GeoPointGroup>
+        GeoPointGroup;
+
+typedef MessageGroups::MessageGroup<MessageGroups::GeometryMsgs,
+                                    MessageGroups::GeometryMsgs::PolygonGroup>
+        PolygonGroup;
+
+typedef MessageGroups::MessageGroup<MessageGroups::GeometryMsgs,
+                                    MessageGroups::GeometryMsgs::PolygonStampedGroup>
+        PolygonStampedGroup;
+
+typedef MessageGroups::MessageGroup<MessageGroups::JSKRecognitionMsgs,
+                                    MessageGroups::JSKRecognitionMsgs::PolygonArrayGroup>
+        PolygonArrayGroup;
+
+} // end namespace MessageGroups
+
+} // end namespace ros_bridge
diff --git a/src/comm/ros_bridge/include/ROSMessageTraits.h b/src/comm/ros_bridge/include/MessageTraits.h
similarity index 51%
rename from src/comm/ros_bridge/include/ROSMessageTraits.h
rename to src/comm/ros_bridge/include/MessageTraits.h
index 8cd20f7b7aec40330cc53d85f151ff3d821cbbf0..4c54c9819f64c4627884c6731760bd41b4077eb5 100644
--- a/src/comm/ros_bridge/include/ROSMessageTraits.h
+++ b/src/comm/ros_bridge/include/MessageTraits.h
@@ -1,6 +1,6 @@
 #pragma once
 
-namespace ros_bridge {
+namespace ROSBridge {
 namespace traits {
 
 
@@ -17,6 +17,22 @@ class HasMemberZ
     template <typename C> static Small test( typeof(&C::z) ) ;
     template <typename C> static Big test(...);
 
+public:
+    enum { value = sizeof(test<T>(0)) == sizeof(Small) };
+};
+//! @brief Checks if class T has a member function SetZ.
+//!
+//! Checks if class T has a member function SetZ. E.g if the class Point has a member function
+//! SetZ than HasMemberSetZ<Point>::value is equal to 1 and 0 either.
+template <typename T>
+class HasMemberSetZ
+{
+    typedef char Small;
+    struct Big { char x[2]; };
+
+    template <typename C> static Small test( typeof(&C::SetZ) ) ;
+    template <typename C> static Big test(...);
+
 public:
     enum { value = sizeof(test<T>(0)) == sizeof(Small) };
 };
@@ -38,6 +54,23 @@ public:
     enum { value = sizeof(test<T>(0)) == sizeof(Small) };
 };
 
+//! @brief Checks if class T has a member function SetLabels.
+//!
+//! Checks if class T has a member function SetLabels. E.g if the class T has a member function
+//! SetLabels than HasMemberSetLabels<T>::value is equal to 1 and 0 either.
+template <typename T>
+class HasMemberSetLabels
+{
+    typedef char Small;
+    struct Big { char x[2]; };
+
+    template <typename C> static Small test( typeof(&C::SetLabels) ) ;
+    template <typename C> static Big test(...);
+
+public:
+    enum { value = sizeof(test<T>(0)) == sizeof(Small) };
+};
+
 //! @brief Checks if class T has a member function labels.
 //!
 //! Checks if class T has a member function likelihood. E.g if the class T has a member function
@@ -55,7 +88,24 @@ public:
     enum { value = sizeof(test<T>(0)) == sizeof(Small) };
 };
 
-//! @brief Checks if class T has a member function labels.
+//! @brief Checks if class T has a member function setLikelihood.
+//!
+//! Checks if class T has a member function setLikelihood. E.g if the class T has a member function
+//! setLikelihood than HasMemberSetLikelihood<T>::value is equal to 1 and 0 either.
+template <typename T>
+class HasMemberSetLikelihood
+{
+    typedef char Small;
+    struct Big { char x[2]; };
+
+    template <typename C> static Small test( typeof(&C::setLikelihood) ) ;
+    template <typename C> static Big test(...);
+
+public:
+    enum { value = sizeof(test<T>(0)) == sizeof(Small) };
+};
+
+//! @brief Checks if class T has a member function header.
 //!
 //! Checks if class T has a member function header. E.g if the class T has a member function
 //! header than HasMemberHeader<T>::value is equal to 1 and 0 either.
@@ -72,6 +122,23 @@ public:
     enum { value = sizeof(test<T>(0)) == sizeof(Small) };
 };
 
+//! @brief Checks if class T has a member function setHeader.
+//!
+//! Checks if class T has a member function setHeader. E.g if the class T has a member function
+//! setHeader than HasMemberSetHeader<T>::value is equal to 1 and 0 either.
+template <typename T>
+class HasMemberSetHeader
+{
+    typedef char Small;
+    struct Big { char x[2]; };
+
+    template <typename C> static Small test( typeof(&C::setHeader) ) ;
+    template <typename C> static Big test(...);
+
+public:
+    enum { value = sizeof(test<T>(0)) == sizeof(Small) };
+};
+
 //! @brief Checks if class T has a member function altitude.
 //!
 //! Checks if class T has a member function altitude. E.g if the class T has a member function
@@ -89,6 +156,23 @@ public:
     enum { value = sizeof(test<T>(0)) == sizeof(Small) };
 };
 
+//! @brief Checks if class T has a member function setAltitude.
+//!
+//! Checks if class T has a member function setAltitude. E.g if the class T has a member function
+//! setAltitude than HasMemberSetAltitude<T>::value is equal to 1 and 0 either.
+template <typename T>
+class HasMemberSetAltitude
+{
+    typedef char Small;
+    struct Big { char x[2]; };
+
+    template <typename C> static Small test( typeof(&C::setAltitude) ) ;
+    template <typename C> static Big test(...);
+
+public:
+    enum { value = sizeof(test<T>(0)) == sizeof(Small) };
+};
+
 //! @brief Selects the Type T or U depending on flag and stores it inside Result.
 template <int flag, typename T, typename U>
 struct Select {typedef T Result;};
diff --git a/src/comm/ros_bridge/include/ROSCommunicator.h b/src/comm/ros_bridge/include/ROSCommunicator.h
new file mode 100644
index 0000000000000000000000000000000000000000..6bf821382fb1ee7538216101d7a610a26716cbf2
--- /dev/null
+++ b/src/comm/ros_bridge/include/ROSCommunicator.h
@@ -0,0 +1,17 @@
+#pragma once
+
+#include "ros_bridge/include/MessageBaseClass.h"
+
+typedef ROSBridge::MessageBaseClass<std::string> ROSMsg;
+
+class ROSCommunicator
+{
+public:
+  explicit ROSCommunicator() {}
+
+    void send(const ROSMsg *msg);
+
+
+public :
+    virtual void receive(ROSMsg *msg) = 0;
+};
diff --git a/src/comm/ros_bridge/include/TypeFactory.h b/src/comm/ros_bridge/include/TypeFactory.h
new file mode 100644
index 0000000000000000000000000000000000000000..edb6916407e32316c8804d63111d3ef4db3bbc74
--- /dev/null
+++ b/src/comm/ros_bridge/include/TypeFactory.h
@@ -0,0 +1,116 @@
+#pragma once
+#pragma once
+
+#include "ros_bridge/rapidjson/include/rapidjson/document.h"
+#include "ros_bridge/include/JsonMethodes.h"
+#include "ros_bridge/include/MessageBaseClass.h"
+#include "utilities.h"
+#include "ros_bridge/include/MessageTraits.h"
+
+#include "ros_bridge/include/MessageGroups.h"
+
+#include "boost/type_traits.hpp"
+#include "boost/type_traits/is_base_of.hpp"
+
+namespace ROSBridge {
+
+//!
+//! \brief The TypeFactory class is used to create C++ representatives of ROS messages.
+//!
+//! 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;
+public:
+
+    TypeFactory(){}
+
+    //!
+    //! \brief Creates a \class rapidjson::Document document containing a ROS mesage from \p msg.
+    //!
+    //! Creates a \class rapidjson::Document document containing a ROS message from \p msg.
+    //! A compile-time error will occur, if \p msg belongs to the \struct EmptyGroup or is
+    //! not derived from \class MessageBaseClass.
+    //! \param msg Instance of a \class MessageBaseClass subclass belonging to a ROSMessageGroup.
+    //! \return rapidjson::Document document containing a ROS message.
+    template <class T>
+    bool create(const rapidjson::Document &doc, T &type){
+        static_assert(boost::is_base_of<ROSMsg, T>::value, "Type of msg must be derived from MessageBaseClass.");
+        static_assert(!::boost::is_same<typename T::Group, MessageGroups::EmptyGroup>::value,
+                "msg belongs to group EmptyGroup (not allowed). Please specify Group (typedef MessageGroup Group)");
+        return _create(doc, type, Type2Type<typename T::Group>());
+    }
+
+private:
+
+    // ===========================================================================
+    // EmptyGroup and not implemented Groups
+    // ===========================================================================
+    template<class U, class V>
+    bool _create(const rapidjson::Document &doc, U &type, Type2Type<V>){
+        (void)type;
+        (void)doc;
+        assert(false); // Implementation missing for group U::Group!
+    }
+
+    // ===========================================================================
+    // Point32Group
+    // ===========================================================================
+    template<class U>
+    bool _create(const rapidjson::Document &doc, U &type, Type2Type<MessageGroups::Point32Group>){
+        using namespace ROSBridge;
+        bool ret = JsonMethodes::Point32::fromJson(doc, type);
+        assert(ret);
+        return ret;
+    }
+
+    // ===========================================================================
+    // GeoPointGroup
+    // ===========================================================================
+    template<class U>
+    bool _create(const rapidjson::Document &doc, U &type, Type2Type<MessageGroups::GeoPointGroup>){
+        using namespace ROSBridge;
+        bool ret = JsonMethodes::GeoPoint::fromJson(doc, type);
+        assert(ret);
+        return ret;
+    }
+
+    // ===========================================================================
+    // PolygonGroup
+    // ===========================================================================
+    template<class U>
+    bool _create(const rapidjson::Document &doc, U &type, Type2Type<MessageGroups::PolygonGroup>){
+        using namespace ROSBridge;
+        bool ret = JsonMethodes::Polygon::fromJson(doc, type);
+        assert(ret);
+        return ret;
+    }
+
+    // ===========================================================================
+    // PolygonStampedGroup
+    // ===========================================================================
+    template<class U>
+    bool _create(const rapidjson::Document &doc, U &type, Type2Type<MessageGroups::PolygonStampedGroup>){
+        using namespace ROSBridge;
+        bool ret = JsonMethodes::PolygonStamped::fromJson(doc, type);
+        assert(ret);
+        return ret;
+    }
+
+    // ===========================================================================
+    // PolygonArrayGroup
+    // ===========================================================================
+    template<class U>
+    bool _create(const rapidjson::Document &doc, U &type, Type2Type<MessageGroups::PolygonArrayGroup>){
+        using namespace ROSBridge;
+        bool ret = JsonMethodes::PolygonArray::fromJson(doc, type);
+        assert(ret);
+        return ret;
+    }
+
+
+};
+
+} // end namespace ros_bridge
diff --git a/src/comm/ros_bridge/src/ROSCommunicator.cpp b/src/comm/ros_bridge/src/ROSCommunicator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fbd134ee58fdeee4293ae46b146d7fb00194a513
--- /dev/null
+++ b/src/comm/ros_bridge/src/ROSCommunicator.cpp
@@ -0,0 +1 @@
+#include "ros_bridge/include/ROSCommunicator.h"