merged with phil

deploy/AppImageCommitHash

-bd79747292138b2ea0

deploy/appImageCommand.txt

-./create_linux_appimage.sh /home/valentin/Desktop/drones/qgroundcontrol/ /home/valentin/Desktop/drones/build-qgroundcontrol-Desktop_Qt_5_11_3_GCC_64bit-Release/release/
+./create_linux_appimage.sh ~/Desktop/drones/qgroundcontrol ~/Desktop/drones/build-qgroundcontrol-Desktop_Qt_5_11_3_GCC_64bit-Release/release

deploy/create_linux_appimage.sh

@@ -66,19 +66,19 @@ dpkg -x libts-0.0-0_1.0-11_amd64.deb libts
 cp -L libts/usr/lib/x86_64-linux-gnu/libts-0.0.so.0 ${APPDIR}/usr/lib/x86_64-linux-gnu/
 
 # copy libortools.so, etc...
-cp -L ${QGC_SRC}/libs/or-tools-src-ubuntu/lib/* ${APPDIR}/usr/lib/x86_64-linux-gnu/
+cp -L ${QGC_SRC}/libs/or-tools-src-ubuntu/lib/* ${APPDIR}/usr/lib/x86_64-linux-gnu/ || { echo "libortools.so not found"; exit  1; }
 
-# copy libGeographic.so
-cp -L /usr/lib/x86_64-linux-gnu/libGeographic.so ${APPDIR}/usr/lib/x86_64-linux-gnu/
+# copy libGeographic.so.17
+cp -L /usr/lib/x86_64-linux-gnu/libGeographic.so.17 ${APPDIR}/usr/lib/x86_64-linux-gnu/ || { echo "libGeographic.so.17 not found"; exit  1; }
 
 # copy boost
-cp -L /usr/lib/x86_64-linux-gnu/libboost_system.so.1.65.1 ${APPDIR}/usr/lib/x86_64-linux-gnu/
+cp -L /usr/lib/x86_64-linux-gnu/libboost_system.so.1.65.1 ${APPDIR}/usr/lib/x86_64-linux-gnu/ || { echo "libboost_system.so.1.65.1 not found"; exit  1; }
 
 # copy libcrypto
-cp -L /usr/lib/x86_64-linux-gnu/libcrypto.so.1.1 ${APPDIR}/usr/lib/x86_64-linux-gnu/
+cp -L /usr/lib/x86_64-linux-gnu/libcrypto.so.1.1 ${APPDIR}/usr/lib/x86_64-linux-gnu/ || { echo "libcrypto.so.1.1 not found"; exit  1; }
 
 # copy libSDL2
-cp -L /usr/lib/x86_64-linux-gnu/libSDL2-2.0.so.0 ${APPDIR}/usr/lib/x86_64-linux-gnu/
+cp -L /usr/lib/x86_64-linux-gnu/libSDL2-2.0.so.0 ${APPDIR}/usr/lib/x86_64-linux-gnu/ || { echo "libSDL2-2.0.so.0 not found"; exit  1; }
 
 
 # copy QGroundControl release into appimage

qgroundcontrol.pro

@@ -27,11 +27,12 @@ QGCROOT  = $$PWD
 
 DebugBuild {
     DESTDIR  = $${OUT_PWD}/debug
-    #DEFINES += DEBUG
-    #DEFINES += ROS_BRIDGE_CLIENT_DEBUG
+    DEFINES += DEBUG
+    #DEFINES += ROS_BRIDGE_DEBUG
 }
 else {
     DESTDIR  = $${OUT_PWD}/release
+    #DEFINES += ROS_BRIDGE_DEBUG
     DEFINES += NDEBUG
 }
 
@@ -438,13 +439,15 @@ HEADERS += \
     src/Wima/Snake/PolygonArray.h \
     src/Wima/Snake/QNemoHeartbeat.h \
     src/Wima/Snake/QNemoProgress.h \
-    src/Wima/Snake/QtROSJsonFactory.h \
-    src/Wima/Snake/QtROSTypeFactory.h \
+    src/Wima/Snake/QNemoProgress.h \
+    src/Wima/Snake/SnakeTiles.h \
+    src/Wima/Snake/SnakeTilesLocal.h \
     src/Wima/Snake/SnakeWorker.h \
     src/Wima/WaypointManager/AreaInterface.h \
     src/Wima/WaypointManager/DefaultManager.h \
     src/Wima/WaypointManager/GenericWaypointManager.h \
     src/Wima/Geometry/WimaPolygonArray.h \
+    src/Wima/Snake/snaketile.h \
     src/Wima/WaypointManager/RTLManager.h \
     src/Wima/WaypointManager/Settings.h \
     src/Wima/WaypointManager/Slicer.h \
@@ -481,18 +484,21 @@ HEADERS += \
     src/Wima/Geometry/testplanimetrycalculus.h \
     src/Settings/WimaSettings.h \
     src/QmlControls/QmlObjectVectorModel.h \
-    src/comm/ros_bridge/include/CasePacker.h \
-    src/comm/ros_bridge/include/ComPrivateInclude.h \
-    src/comm/ros_bridge/include/GenericMessages.h \
-    src/comm/ros_bridge/include/JsonMethodes.h \
-    src/comm/ros_bridge/include/MessageTag.h \
-    src/comm/ros_bridge/include/MessageTraits.h \
     src/comm/ros_bridge/include/RosBridgeClient.h \
-    src/comm/ros_bridge/include/ThreadSafeQueue.h \
-    src/comm/ros_bridge/include/TopicPublisher.h \
-    src/comm/ros_bridge/include/TopicSubscriber.h \
-    src/comm/ros_bridge/include/TypeFactory.h \
-    src/comm/ros_bridge/src/PackageBuffer.h \
+    src/comm/ros_bridge/include/com_private.h \
+    src/comm/ros_bridge/include/message_traits.h \
+    src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h \
+    src/comm/ros_bridge/include/messages/geometry_msgs/point32.h \
+    src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h \
+    src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h \
+    src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h \
+    src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h \
+    src/comm/ros_bridge/include/messages/nemo_msgs/progress.h \
+    src/comm/ros_bridge/include/messages/std_msgs/header.h \
+    src/comm/ros_bridge/include/server.h \
+    src/comm/ros_bridge/include/messages/std_msgs/time.h \
+    src/comm/ros_bridge/include/topic_publisher.h \
+    src/comm/ros_bridge/include/topic_subscriber.h \
     src/comm/utilities.h
 SOURCES += \
     src/Snake/clipper/clipper.cpp \
@@ -510,11 +516,21 @@ SOURCES += \
     src/Wima/WaypointManager/Slicer.cpp \
     src/Wima/WaypointManager/Utils.cpp \
     src/Wima/WimaBridge.cc \
-    src/comm/ros_bridge/include/ComPrivateInclude.cpp \
-    src/comm/ros_bridge/include/MessageTag.cpp \
-    src/comm/ros_bridge/include/TopicPublisher.cpp \
-    src/comm/ros_bridge/include/TopicSubscriber.cpp \
-    src/comm/ros_bridge/src/CasePacker.cpp \
+    src/comm/ros_bridge/include/RosBridgeClient.cpp \
+    src/comm/ros_bridge/include/com_private.cpp \
+    src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.cpp \
+    src/comm/ros_bridge/include/messages/geometry_msgs/point32.cpp \
+    src/comm/ros_bridge/include/messages/geometry_msgs/polygon.cpp \
+    src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.cpp \
+    src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.cpp \
+    src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.cpp \
+    src/comm/ros_bridge/include/messages/nemo_msgs/progress.cpp \
+    src/comm/ros_bridge/include/messages/std_msgs/header.cpp \
+    src/comm/ros_bridge/include/messages/std_msgs/time.cpp \
+    src/comm/ros_bridge/include/server.cpp \
+    src/comm/ros_bridge/include/topic_publisher.cpp \
+    src/comm/ros_bridge/include/topic_subscriber.cpp \
+    src/Wima/Snake/snaketile.cpp \
     src/api/QGCCorePlugin.cc \
     src/api/QGCOptions.cc \
     src/api/QGCSettings.cc \
@@ -545,7 +561,7 @@ SOURCES += \
     src/Wima/Geometry/testplanimetrycalculus.cpp \
     src/Settings/WimaSettings.cc \
     src/QmlControls/QmlObjectVectorModel.cc \
-    src/comm/ros_bridge/src/ROSBridge.cpp
+    src/comm/ros_bridge/src/ros_bridge.cpp
 
 #
 # Unit Test specific configuration goes here (requires full debug build with all plugins)

src/Settings/Wima.SettingsGroup.json

@@ -18,5 +18,11 @@
     "type":             "uint64",
     "units":            "s",
     "defaultValue":     15
+},
+{
+    "name":             "rosbridgeConnectionString",
+    "shortDescription": "Ros Bridge Connection String (host:port).",
+    "type":             "string",
+    "defaultValue":     "localhost:9090"
 }
 ]

src/Settings/WimaSettings.cc

@@ -11,3 +11,4 @@ DECLARE_SETTINGGROUP(Wima, "Wima")
 DECLARE_SETTINGSFACT(WimaSettings, lowBatteryThreshold)
 DECLARE_SETTINGSFACT(WimaSettings, enableLowBatteryHandling)
 DECLARE_SETTINGSFACT(WimaSettings, minimalRemainingMissionTime)
+DECLARE_SETTINGSFACT(WimaSettings, rosbridgeConnectionString)

src/Settings/WimaSettings.h

@@ -13,4 +13,5 @@ public:
     DEFINE_SETTINGFACT(lowBatteryThreshold)
     DEFINE_SETTINGFACT(enableLowBatteryHandling)
     DEFINE_SETTINGFACT(minimalRemainingMissionTime)
+    DEFINE_SETTINGFACT(rosbridgeConnectionString)
 };

src/Wima/Geometry/GeoPoint3D.cpp

 #include "GeoPoint3D.h"
 
-GeoPoint::GeoPoint(QObject *parent)
+GeoPoint3D::GeoPoint3D(QObject *parent)
     : QObject(parent), ROSGeoPoint() {}
 
-GeoPoint::GeoPoint(double latitude, double longitude, double altitude, QObject *parent)
+GeoPoint3D::GeoPoint3D(double latitude, double longitude, double altitude, QObject *parent)
     : QObject(parent), ROSGeoPoint(latitude, longitude, altitude)
 {}
 
-GeoPoint::GeoPoint(const GeoPoint &p, QObject *parent)
+GeoPoint3D::GeoPoint3D(const GeoPoint3D &p, QObject *parent)
     : QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
 {}
 
-GeoPoint::GeoPoint(const ROSGeoPoint &p, QObject *parent)
+GeoPoint3D::GeoPoint3D(const ROSGeoPoint &p, QObject *parent)
     : QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
 {}
 
-GeoPoint::GeoPoint(const QGeoCoordinate &p, QObject *parent)
+GeoPoint3D::GeoPoint3D(const QGeoCoordinate &p, QObject *parent)
     : QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
 {}
 
-GeoPoint *GeoPoint::Clone() const
-{
-    return new GeoPoint(*this);
-}
-
-GeoPoint &GeoPoint::operator=(const GeoPoint &p)
+GeoPoint3D &GeoPoint3D::operator=(const GeoPoint3D &p)
 {
     this->setLatitude(p.latitude());
     this->setLongitude(p.longitude());
@@ -32,7 +27,7 @@ GeoPoint &GeoPoint::operator=(const GeoPoint &p)
 
     return *this;
 }
-GeoPoint &GeoPoint::operator=(const QGeoCoordinate &p)
+GeoPoint3D &GeoPoint3D::operator=(const QGeoCoordinate &p)
 {
     this->setLatitude(p.latitude());
     this->setLongitude(p.longitude());

src/Wima/Geometry/GeoPoint3D.h

 #pragma once
 
-#include "ros_bridge/include/JsonMethodes.h"
-#include "ros_bridge/include/MessageBaseClass.h"
-#include "ros_bridge/include/GenericMessages.h"
-
 #include <QGeoCoordinate>
 #include <QObject>
 
+#include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
+
 
-typedef ROSBridge::MessageBaseClass ROSMsg;
-typedef ROSBridge::GenericMessages::GeographicMsgs::GeoPoint ROSGeoPoint;
-namespace MsgGroups = ROSBridge::MessageGroups;
-class GeoPoint : public QObject, public ROSGeoPoint
+typedef ros_bridge::messages::geographic_msgs::geo_point::GeoPoint ROSGeoPoint;
+class GeoPoint3D : public QObject, public ROSGeoPoint
 {
     Q_OBJECT
 public:
-    typedef MsgGroups::GeoPointGroup Group;
 
-    GeoPoint(QObject *parent = nullptr);
-    GeoPoint(double latitude,
+    GeoPoint3D(QObject *parent = nullptr);
+    GeoPoint3D(double latitude,
                double longitude,
                double altitude,
                QObject *parent = nullptr);
-    GeoPoint(const GeoPoint& p,
+    GeoPoint3D(const GeoPoint3D& p,
                QObject *parent = nullptr);
-    GeoPoint(const ROSGeoPoint& p,
+    GeoPoint3D(const ROSGeoPoint& p,
                QObject *parent = nullptr);
-    GeoPoint(const QGeoCoordinate& p,
+    GeoPoint3D(const QGeoCoordinate& p,
                QObject *parent = nullptr);
 
-    virtual GeoPoint *Clone() const override;
-    GeoPoint &operator=(const GeoPoint&p);
-    GeoPoint &operator=(const QGeoCoordinate&p);
+    GeoPoint3D &operator=(const GeoPoint3D&p);
+    GeoPoint3D &operator=(const QGeoCoordinate&p);
 
 };
 

src/Wima/Geometry/Polygon2D.h

@@ -3,41 +3,30 @@
 #include <QPolygonF>
 #include <QPointF>
 
-#include "ros_bridge/include/MessageGroups.h"
-#include "ros_bridge/include/GenericMessages.h"
-#include "ros_bridge/include/MessageBaseClass.h"
-#include "ros_bridge/include/JsonMethodes.h"
+#include "ros_bridge/include/messages/geometry_msgs/polygon_stamped.h"
 
-namespace MsgGroupsNS = ROSBridge::MessageGroups;
-namespace PolyStampedNS = ROSBridge::JsonMethodes::GeometryMsgs::PolygonStamped;
-
-typedef ROSBridge::MessageBaseClass ROSMsg;
+namespace polygon_stamped = ros_bridge::messages::geometry_msgs::polygon_stamped;
 
 template <class PointType = QPointF, template <class, class...> class ContainerType = QVector>
-class Polygon2DTemplate : public ROSMsg{ //
-    typedef ROSBridge::GenericMessages::GeometryMsgs::GenericPolygon<PointType, ContainerType> Poly;
+class Polygon2DTemplate{ //
+    typedef ros_bridge::messages::geometry_msgs::polygon::GenericPolygon<PointType, ContainerType> Polygon;
 public:
-    typedef MsgGroupsNS::PolygonStampedGroup Group; // has no header
-
     Polygon2DTemplate(){}
-    Polygon2DTemplate(const Polygon2DTemplate &other) : ROSMsg(), _polygon(other._polygon){}
+    Polygon2DTemplate(const Polygon2DTemplate &other) : _polygon(other._polygon){}
 
     Polygon2DTemplate& operator=(const Polygon2DTemplate& other) {
         this->_polygon = other._polygon;
         return *this;
     }
 
-    const Poly &polygon() const {return _polygon;}
-          Poly &polygon()       {return _polygon;}
+    const Polygon &polygon() const {return _polygon;}
+          Polygon &polygon()       {return _polygon;}
 
   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;
+    Polygon _polygon;
 
 };
 

src/Wima/Geometry/PolygonArray.h

 #pragma once
 
-#include <QObject>
+#include <QString>
 
-#include "ros_bridge/include/MessageBaseClass.h"
-
-typedef ROSBridge::MessageBaseClass ROSMsgBase;
 template <class PolygonType, template <class,class...> class ContainerType >
-class PolygonArray : public ROSMsgBase, public ContainerType<PolygonType> {
+class PolygonArray : public ContainerType<PolygonType> {
 public:
-    explicit PolygonArray() : ROSMsgBase(), ContainerType<PolygonType>() {}
+    explicit PolygonArray() : ContainerType<PolygonType>() {}
     PolygonArray(const PolygonArray &other) : ContainerType<PolygonType>(other) {}
 
     QString type() const override {return "PolygonArray";}

src/Wima/Geometry/WimaPolygonArray.h

 #pragma once
-#include "ros_bridge/include/MessageBaseClass.h"
-#include "ros_bridge/include/MessageGroups.h"
 
 #include "QmlObjectListModel.h"
 
 #include <QVector>
 #include <QString>
 
-typedef ROSBridge::MessageBaseClass 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
+template <class PolygonType, template <class, class...> class ContainerType = QVector>
+class WimaPolygonArray
 {
 public:
-    typedef GroupType Group;
     WimaPolygonArray() {}
     WimaPolygonArray(const WimaPolygonArray &other) :
-        ROSMsg()
-      , _polygons(other._polygons), _dirty(true)
+        _polygons(other._polygons), _dirty(true)
     {}
-
-    virtual WimaPolygonArray *Clone() const override{
-        return new WimaPolygonArray(*this);
+    ~WimaPolygonArray(){
+        _objs.clearAndDeleteContents();
     }
 
     class QmlObjectListModel *QmlObjectListModel(){
@@ -51,9 +43,9 @@ public:
 
 private:
     void _updateObjects(void){
-        _objs.clear();
+        _objs.clearAndDeleteContents();
         for (long i=0; i<_polygons.size(); ++i){
-            _objs.append(&_polygons[i]);
+            _objs.append(new PolygonType(_polygons[i]));
         }
     }
 

src/Wima/Snake/PolygonArray.h

 #pragma once
 
-#include "ros_bridge/include/MessageGroups.h"
 #include "Wima/Geometry/Polygon2D.h"
 #include "Wima/Geometry/WimaPolygonArray.h"
 
-namespace  MsgGroups = ROSBridge::MessageGroups;
-typedef WimaPolygonArray<Polygon2D, QVector, MsgGroups::PolygonArrayGroup> PolygonArray;
+typedef WimaPolygonArray<Polygon2D, QVector> SnakeTilesLocal;

src/Wima/Snake/QNemoHeartbeat.h

 #pragma once
 
-#include "ros_bridge/include/GenericMessages.h"
+#include "ros_bridge/include/messages/nemo_msgs/heartbeat.h"
 
-using QNemoHeartbeat = ROSBridge::GenericMessages::NemoMsgs::Heartbeat;
+using QNemoHeartbeat = ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat;

src/Wima/Snake/QNemoProgress.h

 #pragma once
-#include <QVector>
-#include <QObject>
-
-#include "ros_bridge/include/GenericMessages.h"
 
-namespace NemoMsgs = ROSBridge::GenericMessages::NemoMsgs;
-typedef NemoMsgs::GenericProgress<int, QVector> QNemoProgress;
+#include <QVector>
 
+#include "ros_bridge/include/messages/nemo_msgs/progress.h"
 
+namespace nemo = ros_bridge::messages::nemo_msgs;
+typedef nemo::progress::GenericProgress<int, QVector> QNemoProgress;

src/Wima/Snake/QtROSJsonFactory.h

-#pragma once
-#include "ros_bridge/include/JsonFactory.h"
-#include <QString>
-
-typedef ROSBridge::GenericJsonFactory<> QtROSJsonFactory;

src/Wima/Snake/QtROSTypeFactory.h

-#pragma once
-#include "ros_bridge/include/TypeFactory.h"
-#include <QString>
-
-typedef ROSBridge::TypeFactory QtROSTypeFactory;

src/Wima/WimaController.h

@@ -35,18 +35,14 @@
 #include "Snake/QNemoProgress.h"
 #include "Snake/QNemoHeartbeat.h"
 
-#include "ros_bridge/include/ROSBridge.h"
+#include "ros_bridge/include/ros_bridge.h"
 
 #include "WaypointManager/DefaultManager.h"
 #include "WaypointManager/RTLManager.h"
 
-#include <map>
+#include "utilities.h"
 
-#define CHECK_BATTERY_INTERVAL 1000 // ms
-#define SMART_RTL_MAX_ATTEMPTS 3 // times
-#define SMART_RTL_ATTEMPT_INTERVAL 200 // ms
-#define EVENT_TIMER_INTERVAL 50 // ms
-#define SNAKE_EVENT_LOOP_INTERVAL 1000 // ms
+#include <map>
 
 
 using namespace snake;
@@ -59,7 +55,7 @@ class WimaController : public QObject
 
     enum FileType {WimaFile, PlanFile};
 
-    typedef QScopedPointer<ROSBridge::ROSBridge> ROSBridgePtr;
+    typedef QScopedPointer<ros_bridge::ROSBridge> ROSBridgePtr;
 
 public:
 
@@ -338,9 +334,10 @@ private slots:
     void    _smartRTLCleanUp                    (bool flying);
     // Snake.
     void _setSnakeCalcInProgress             (bool inProgress);
-    void _updateSnakeData                    ();
+    void _snakeStoreWorkerResults            ();
     void _initStartSnakeWorker               ();
     void _switchSnakeManager                 (QVariant variant);
+    bool _doTopicServiceSetup();
     // Periodic tasks.
     void _eventTimerHandler                  (void);
     // Waypoint manager handling.
@@ -396,8 +393,6 @@ private:
 
     // Waypoint statistics.
     double                       _measurementPathLength; // the lenght of the phase in meters
-//    double                       _phaseDistance; // the lenth in meters of the current phase
-//    double                       _phaseDuration; // the phase duration in seconds
 
     // Snake
     bool                    _snakeCalcInProgress;
@@ -410,11 +405,14 @@ private:
     QNemoHeartbeat          _nemoHeartbeat; // measurement progress
     int                     _fallbackStatus;
     ROSBridgePtr            _pRosBridge;
-    bool                    _bridgeSetupDone;
     static StatusMap        _nemoStatusMap;
+    bool                    _topicServiceSetupDone;
 
     // Periodic tasks.
-    QTimer                  _eventTimer;
+    QTimer      _eventTimer;
+    EventTicker _batteryLevelTicker;
+    EventTicker _snakeTicker;
+    EventTicker _nemoTimeoutTicker;
 
 };
 

src/comm/ros_bridge/include/CasePacker.h

-#pragma once
-#include "ros_bridge/include/MessageBaseClass.h"
-#include "ros_bridge/include/MessageTag.h"
-#include "ros_bridge/include/TypeFactory.h"
-#include "ros_bridge/include/JsonFactory.h"
-
-#include <memory>
-#include "rapidjson/include/rapidjson/document.h"
-
-namespace ROSBridge {
-
-class CasePacker
-{
-    typedef MessageTag Tag;
-    typedef rapidjson::Document JsonDoc;
-    typedef std::unique_ptr<JsonDoc> JsonDocUPtr;
-public:
-    CasePacker() = delete;
-    CasePacker(TypeFactory *typeFactory, JsonFactory *jsonFactory);
-
-    template<class T>
-    JsonDocUPtr pack(const T &msg, const std::string &topic) const
-    {
-        JsonDocUPtr docPt(_jsonFactory->create(msg));
-        std::string messageType = T::Group::messageType();
-        addTag(docPt, topic, messageType.c_str());
-        return docPt;
-    }
-
-    template<class T>
-    bool unpack(JsonDocUPtr &pDoc, T &msg) const {
-        removeTag(pDoc);
-        return _typeFactory->create(*pDoc.get(), msg);
-    }
-
-
-    bool getTag(const JsonDocUPtr &pDoc, Tag &tag) const;
-    void addTag        (JsonDocUPtr &doc,
-                         const std::string &topic,
-                         const std::string &messageType) const;
-    void addTag        (JsonDocUPtr &doc, const Tag &tag) const;
-    void removeTag     (JsonDocUPtr &pDoc) const;
-    bool getTopic      (const JsonDocUPtr &pDoc, std::string &topic) const;
-    bool getMessageType(const JsonDocUPtr &pDoc, std::string &messageType) const;
-
-    static const char* topicKey;
-    static const char* messageTypeKey;
-
-private:
-    TypeFactory *_typeFactory;
-    JsonFactory *_jsonFactory;
-};
-}
-

src/comm/ros_bridge/include/ComPrivateInclude.cpp

-#include "ros_bridge/include/ComPrivateInclude.h"
-#include <functional>
-
-std::size_t ROSBridge::ComPrivate::getHash(const std::string &str)
-{
-    std::hash<std::string> hash;
-    return hash(str);
-}
-
-std::size_t ROSBridge::ComPrivate::getHash(const char *str)
-{
-    return ROSBridge::ComPrivate::getHash(std::string(str));
-}

src/comm/ros_bridge/include/JsonFactory.h

-#pragma once
-
-#include "ros_bridge/rapidjson/include/rapidjson/document.h"
-#include "ros_bridge/include/JsonMethodes.h"
-#include "MessageBaseClass.h"
-#include "utilities.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"
-
-namespace ROSBridge {
-
-class StdHeaderPolicy;
-
-//!
-//! \brief The JsonFactory class is used to create ROS messages.
-//!
-//! 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 HeaderPolicy = StdHeaderPolicy>
-class GenericJsonFactory : public HeaderPolicy
-{
-    typedef MessageBaseClass ROSMsg;
-public:
-
-    GenericJsonFactory() : HeaderPolicy() {}
-
-    //!
-    //! \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>
-    rapidjson::Document *create(const T &msg){
-        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>());
-    }
-
-private:
-
-    // ===========================================================================
-    // EmptyGroup and not implemented Groups
-    // ===========================================================================
-    template<class U, class V>
-    rapidjson::Document *_create(const U &msg, Type2Type<V>){
-        (void)msg;
-        assert(false); // Implementation missing for group U::Group!
-        return nullptr;
-    }
-
-    // ===========================================================================
-    // Point32Group
-    // ===========================================================================
-    template<class U>
-    rapidjson::Document *_create(const U &msg, Type2Type<MessageGroups::Point32Group>){
-        using namespace ROSBridge;
-        rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
-        bool ret = JsonMethodes::GeometryMsgs::Point32::toJson<_Float32>(msg, *doc, doc->GetAllocator());
-        assert(ret);
-        (void)ret;
-
-        return doc;
-    }
-
-    // ===========================================================================
-    // GeoPointGroup
-    // ===========================================================================
-    template<class U>
-    rapidjson::Document *_create(const U &msg, Type2Type<MessageGroups::GeoPointGroup>){
-        using namespace ROSBridge;
-        rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
-        bool ret = JsonMethodes::GeographicMsgs::GeoPoint::toJson(msg, *doc, doc->GetAllocator());
-        assert(ret);
-        (void)ret;
-
-        return doc;
-    }
-
-    // ===========================================================================
-    // PolygonGroup
-    // ===========================================================================
-    template<class U>
-    rapidjson::Document *_create(const U &msg, Type2Type<MessageGroups::PolygonGroup>){
-        using namespace ROSBridge;
-        rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
-        bool ret = JsonMethodes::GeometryMsgs::Polygon::toJson(msg, *doc, doc->GetAllocator());
-        assert(ret);
-        (void)ret;
-
-        return doc;
-    }
-
-    // ===========================================================================
-    // PolygonStampedGroup
-    // ===========================================================================
-    template<class U>
-    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>());
-    }
-
-    template<class U, int k>
-    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::GeometryMsgs::PolygonStamped::toJson(msg, *doc, doc->GetAllocator());
-        assert(ret);
-        (void)ret;
-
-        return doc;
-
-    }
-
-    template<class U>
-    rapidjson::Document *_createPolygonStamped(const U &msg, Int2Type<0>){// U has no member header(), generate one on the fly.
-        using namespace ROSBridge;
-        GenericMessages::StdMsgs::Header header(HeaderPolicy::header(msg));
-        rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
-        bool ret = JsonMethodes::GeometryMsgs::PolygonStamped::toJson(msg.polygon(), header, *doc, doc->GetAllocator());
-        assert(ret);
-        (void)ret;
-
-        return doc;
-    }
-
-    // ===========================================================================
-    // PolygonArrayGroup
-    // ===========================================================================
-    template<class U>
-    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>());
-    }
-
-    template<class U, int k>
-    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::JSKRecognitionMsgs::PolygonArray::toJson(msg, *doc, doc->GetAllocator());
-        assert(ret);
-        (void)ret;
-
-        return doc;
-
-    }
-
-    template<class U>
-    rapidjson::Document *_createPolygonArray(const U &msg, Int2Type<0>){// U has no member header(), generate one on the fly.
-        using namespace ROSBridge;
-        GenericMessages::StdMsgs::Header header(HeaderPolicy::header(msg));
-        rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
-        bool ret = JsonMethodes::JSKRecognitionMsgs::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;
-    }
-
-    // ===========================================================================
-    // HeartbeatGroup
-    // ===========================================================================
-    template<class U>
-    rapidjson::Document *_create(const U &msg, Type2Type<MessageGroups::HeartbeatGroup>){
-        using namespace ROSBridge;
-        rapidjson::Document *doc = new rapidjson::Document(rapidjson::kObjectType);
-        bool ret = JsonMethodes::NemoMsgs::Heartbeat::toJson(msg, *doc, doc->GetAllocator());
-        assert(ret);
-        (void)ret;
-
-        return doc;
-    }
-};
-
-class StdHeaderPolicy
-{
-    typedef ROSBridge::GenericMessages::StdMsgs::Header Header;
-    typedef ROSBridge::GenericMessages::StdMsgs::Time Time;
-public:
-    StdHeaderPolicy():_seq(-1){}
-
-    //!
-    //! \brief header Returns the header belonging to msg.
-    //! \return Returns the header belonging to msg.
-    template<class T>
-    Header header(const T &msg) {
-        return Header(std::uint32_t(++_seq), time(msg), "/map");
-    }
-
-
-    //!
-    //! \brief time Returns the current time.
-    //! \return Returns the current time.    
-    template<class T>
-    Time time(const T &msg) {
-        (void)msg;
-        return Time(0,0);
-    }
-
-private:
-    long _seq;
-};
-
-typedef GenericJsonFactory<> JsonFactory;
-
-} // end namespace ros_bridge
-
-

src/comm/ros_bridge/include/MessageBaseClass.h

-#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.
-class MessageBaseClass{
-public:
-    typedef MessageGroups::EmptyGroup Group;
-
-    MessageBaseClass() {};
-    virtual ~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;
-};
-typedef MessageBaseClass MsgBase;
-} // namespace ROSBridge
-
-

src/comm/ros_bridge/include/MessageGroups.h

-#pragma once
-
-#include <string>
-
-namespace ROSBridge {
-
-namespace MessageGroups {
-
-typedef std::string StringType;
-
-template<typename Group, typename SubGroup, typename...MoreSubGroups>
-struct MessageGroup {
-    static StringType messageType() {return _full<Group, SubGroup, MoreSubGroups...>();}
-
-    template<typename G, typename SubG, typename...MoreSubG>
-    static StringType _full() {return G::label()+ "/" + _full<SubG, MoreSubG...>(); }
-    template<typename G>
-    static StringType _full() {return G::label(); }
-
-
-    static StringType messageTypeLast() {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(); }
-};
-
-//!
-//! \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 GeographicMsgs {
-
-    static StringType label() {return "geographic_msgs";}
-
-    //!
-    //! \brief The GeoPointGroup struct is used the mark a class as a ROS geographic_msgs/GeoPoint message.
-    struct GeoPointGroup {
-        static StringType label() {return "GeoPoint";}
-    };
-};
-
-struct JSKRecognitionMsgs {
-
-    static StringType label() {return "jsk_recognition_msgs";}
-
-    //!
-    //! \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 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.
-    struct ProgressGroup {
-        static StringType label() {return "Progress";}
-    };
-
-    //!
-    //! \brief The HeartbeatGroup struct is used the mark a class as a ROS nemo_msgs/Heartbeat message.
-    struct HeartbeatGroup {
-        static StringType label() {return "Heartbeat";}
-    };
-};
-
-
-typedef MessageGroup<detail::EmptyGroup,
-                     detail::EmptyGroup>
-        EmptyGroup;
-
-typedef MessageGroups::MessageGroup<MessageGroups::GeometryMsgs,
-                                    MessageGroups::GeometryMsgs::Point32Group>
-        Point32Group;
-
-typedef MessageGroups::MessageGroup<MessageGroups::GeographicMsgs,
-                                    MessageGroups::GeographicMsgs::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;
-
-typedef MessageGroups::MessageGroup<MessageGroups::NemoMsgs,
-                                    MessageGroups::NemoMsgs::ProgressGroup>
-        ProgressGroup;
-
-typedef MessageGroups::MessageGroup<MessageGroups::NemoMsgs,
-                                    MessageGroups::NemoMsgs::HeartbeatGroup>
-        HeartbeatGroup;
-
-} // end namespace MessageGroups
-
-} // end namespace ros_bridge

src/comm/ros_bridge/include/MessageTag.cpp

-#include "MessageTag.h"
-
-MessageTag::MessageTag()
-{
-
-}
-
-MessageTag::MessageTag(const std::string &topic, const std::string &messageType) :
-    _topic(topic)
-  , _messagType(messageType)
-{
-
-}
-
-const std::string &MessageTag::topic() const
-{
-    return _topic;
-}
-
-const std::string &MessageTag::messageType() const
-{
-    return _messagType;
-}
-
-std::string &MessageTag::topic()
-{
-    return _topic;
-}
-
-std::string &MessageTag::messageType()
-{
-    return _messagType;
-}
-
-void MessageTag::setTopic(const std::string &topic)
-{
-    _topic = topic;
-}
-
-void MessageTag::setMessageType(const std::string &messageType)
-{
-    _messagType = messageType;
-}

src/comm/ros_bridge/include/MessageTag.h

-#pragma once
-
-#include <string>
-
-
-class MessageTag {
-public:
-    MessageTag();
-    MessageTag(const std::string &topic, const std::string &messageType);
-
-    const std::string &topic() const;
-    const std::string &messageType() const;
-
-    std::string &topic();
-    std::string &messageType();
-
-    void setTopic(const std::string &topic);
-    void setMessageType(const std::string &messageType);
-
-private:
-   std::string _topic;
-   std::string _messagType;
-};
-typedef MessageTag Tag;

src/comm/ros_bridge/include/ThreadSafeQueue.h

-#pragma once
-
-#include <queue>
-#include <mutex>
-#include <condition_variable>
-
-namespace ROSBridge {
-
-template <class T>
-class ThreadSafeQueue
-{
-public:
-    ThreadSafeQueue();
-    ~ThreadSafeQueue();
-
-    T pop_front();
-
-    void push_back(const T& item);
-    void push_back(T&& item);
-
-    int size();
-    bool empty();
-
-private:
-    std::deque<T> _queue;
-    std::mutex _mutex;
-    std::condition_variable _cond;
-};
-
-template <typename T>
-ThreadSafeQueue<T>::ThreadSafeQueue(){}
-
-template <typename T>
-ThreadSafeQueue<T>::~ThreadSafeQueue(){}
-
-template <typename T>
-T ThreadSafeQueue<T>::pop_front()
-{
-    std::unique_lock<std::mutex> mlock(_mutex);
-    while (_queue.empty())
-    {
-        _cond.wait(mlock);
-    }
-    T t = std::move(_queue.front());
-    _queue.pop_front();
-    return t;
-}
-
-template <typename T>
-void ThreadSafeQueue<T>::push_back(const T& item)
-{
-    std::unique_lock<std::mutex> mlock(_mutex);
-    _queue.push_back(item);
-    mlock.unlock();     // unlock before notificiation to minimize mutex con
-    _cond.notify_one(); // notify one waiting thread
-
-}
-
-template <typename T>
-void ThreadSafeQueue<T>::push_back(T&& item)
-{
-    std::unique_lock<std::mutex> mlock(_mutex);
-    _queue.push_back(std::move(item));
-    mlock.unlock();     // unlock before notificiation to minimize mutex con
-    _cond.notify_one(); // notify one waiting thread
-
-}
-
-template <typename T>
-int ThreadSafeQueue<T>::size()
-{
-    std::unique_lock<std::mutex> mlock(_mutex);
-    int size = _queue.size();
-    mlock.unlock();
-    return size;
-}
-
-} // namespace
-

src/comm/ros_bridge/include/TopicPublisher.cpp

-#include "TopicPublisher.h"
-
-
-
-
-
-struct ROSBridge::ComPrivate::ThreadData
-{
-    const ROSBridge::CasePacker      &casePacker;
-    RosbridgeWsClient                &rbc;
-    ROSBridge::ComPrivate::JsonQueue &queue;
-    std::mutex                       &queueMutex;
-    const std::atomic<bool>          &running;
-    std::condition_variable          &cv;
-};
-
-ROSBridge::ComPrivate::TopicPublisher::TopicPublisher(CasePacker &casePacker,
-                                                      RosbridgeWsClient &rbc) :
-      _running(false)
-    , _casePacker(casePacker)
-    , _rbc(rbc)
-{
-
-}
-
-ROSBridge::ComPrivate::TopicPublisher::~TopicPublisher()
-{
-    this->reset();
-}
-
-void ROSBridge::ComPrivate::TopicPublisher::start()
-{
-    if ( _running.load() )  // start called while thread running.
-        return;
-    _running.store(true);
-    ROSBridge::ComPrivate::ThreadData data{
-                _casePacker,
-                _rbc,
-                _queue,
-                _queueMutex,
-                _running,
-                _cv
-    };
-    _pThread = std::make_unique<std::thread>(&ROSBridge::ComPrivate::transmittLoop, data);
-}
-
-void ROSBridge::ComPrivate::TopicPublisher::reset()
-{
-    if ( !_running.load() )  // stop called while thread not running.
-        return;
-    _running.store(false);
-    _cv.notify_one(); // Wake publisher thread.
-    if ( !_pThread )
-        return;
-    _pThread->join();
-    _queue.clear();
-}
-
-void ROSBridge::ComPrivate::transmittLoop(ROSBridge::ComPrivate::ThreadData data)
-{
-    // Init.
-    ClientMap           clientMap;
-    // Main Loop.
-    while(data.running.load()){
-        std::unique_lock<std::mutex> lk(data.queueMutex);
-        // Check if new data available, wait if not.
-        if (data.queue.empty()){
-            data.cv.wait(lk); // Wait for condition, spurious wakeups don't matter in this case.
-            continue;
-        }
-        // Pop message from queue.
-        JsonDocUPtr pJsonDoc(std::move(data.queue.front()));
-        data.queue.pop_front();
-        lk.unlock();
-
-        // Get tag from Json message and remove it.
-        Tag tag;
-        bool ret = data.casePacker.getTag(pJsonDoc, tag);
-        assert(ret); // Json message does not contain a tag;
-        (void)ret;
-        data.casePacker.removeTag(pJsonDoc);
-
-        // Check if topic must be advertised.
-        // Advertised topics are stored in advertisedTopicsHashList as
-        // a hash.
-        std::string clientName = ROSBridge::ComPrivate::_topicAdvertiserKey
-                               + tag.topic();
-        HashType hash = ROSBridge::ComPrivate::getHash(clientName);
-        auto it = clientMap.find(hash);
-        if ( it == clientMap.end()) { // Need to advertise topic?
-            clientMap.insert(std::make_pair(hash, clientName));
-            std::cout << clientName << ";"
-                      << tag.topic() << ";"
-                      << tag.messageType() << ";" << std::endl;
-            data.rbc.addClient(clientName);
-            data.rbc.advertise(clientName,
-                               tag.topic(),
-                               tag.messageType() );
-        }
-
-        // Publish Json message.
-        data.rbc.publish(tag.topic(), *pJsonDoc.get());
-    } // while loop
-
-    // Tidy up.
-    for (auto& it : clientMap)
-        data.rbc.removeClient(it.second);
-}
-

src/comm/ros_bridge/include/TopicSubscriber.cpp

-#include "TopicSubscriber.h"
-
-
-ROSBridge::ComPrivate::TopicSubscriber::TopicSubscriber(CasePacker &casePacker,
-        RosbridgeWsClient &rbc) :
-    _casePacker(casePacker)
-  , _rbc(rbc)
-  , _running(false)
-{
-
-}
-
-ROSBridge::ComPrivate::TopicSubscriber::~TopicSubscriber()
-{
-    this->reset();
-}
-
-void ROSBridge::ComPrivate::TopicSubscriber::start()
-{
-    _running = true;
-}
-
-void ROSBridge::ComPrivate::TopicSubscriber::reset()
-{
-    if ( _running ){
-        _running = false;
-        {
-            for (std::string clientName : _clientList){
-                _rbc.removeClient(clientName);
-            }
-        }
-        {
-            std::lock_guard<std::mutex> lk(_callbackMapStruct.mutex);
-            _callbackMapStruct.map.clear();
-        }
-        _clientList.clear();
-    }
-}
-
-bool ROSBridge::ComPrivate::TopicSubscriber::subscribe(
-        const char *topic,
-        const std::function<void(JsonDocUPtr)> &callback)
-{
-    if ( !_running )
-        return false;
-
-    std::string clientName = ROSBridge::ComPrivate::_topicSubscriberKey
-                           + std::string(topic);
-    _clientList.push_back(clientName);
-
-    HashType hash = getHash(clientName);
-    {
-        std::lock_guard<std::mutex> lk(_callbackMapStruct.mutex);
-        auto ret = _callbackMapStruct.map.insert(std::make_pair(hash, callback)); //
-        if ( !ret.second )
-            return false; // Topic subscription already present.
-
-    }
-
-    using namespace std::placeholders;
-    auto f = std::bind(&ROSBridge::ComPrivate::subscriberCallback,
-                      hash,
-                      std::ref(_callbackMapStruct),
-                      _1, _2);
-
-//    std::cout << std::endl;
-//    std::cout << "topic subscription" << std::endl;
-//    std::cout << "client name: " << clientName << std::endl;
-//    std::cout << "topic: " << topic << std::endl;
-    {
-        auto start = std::chrono::high_resolution_clock::now();
-        _rbc.addClient(clientName);
-        auto end = std::chrono::high_resolution_clock::now();
-        std::cout  << "add client time: "
-                   << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
-                   << " ms" << std::endl;
-        start = std::chrono::high_resolution_clock::now();
-        _rbc.subscribe(clientName,
-                        topic,
-                        f );
-        end = std::chrono::high_resolution_clock::now();
-        std::cout  << "subscribe time: "
-                   << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
-                   << " ms" << std::endl;
-    }
-
-    return true;
-}
-
-
-using WsClient = SimpleWeb::SocketClient<SimpleWeb::WS>;
-void ROSBridge::ComPrivate::subscriberCallback(
-        const HashType &hash,
-        ROSBridge::ComPrivate::MapStruct &mapWrapper,
-        std::shared_ptr<WsClient::Connection>,
-        std::shared_ptr<WsClient::InMessage> in_message)
-{
-    // Parse document.
-    JsonDoc docFull;
-    docFull.Parse(in_message->string().c_str());
-    if ( docFull.HasParseError() ) {
-        std::cout << "Json document has parse error: "
-                  << in_message->string()
-                  << std::endl;
-        return;
-    } else if (!docFull.HasMember("msg")) {
-        std::cout << "Json document does not contain a message (\"msg\"): "
-                  << in_message->string()
-                  << std::endl;
-        return;
-    }
-
-
-//    std::cout << "Json document: "
-//              << in_message->string()
-//              << std::endl;
-
-    // Search callback.
-    CallbackType callback;
-    {
-        std::lock_guard<std::mutex> lk(mapWrapper.mutex);
-        auto it = mapWrapper.map.find(hash);
-        if (it == mapWrapper.map.end()) {
-            //assert(false); // callback not found
-            return;
-        }
-        callback = it->second;
-    }
-
-    // Extract message and call callback.
-    JsonDocUPtr pDoc(new JsonDoc());
-    pDoc->CopyFrom(docFull["msg"].Move(), docFull.GetAllocator());
-    callback(std::move(pDoc));
-    return;
-
-}

src/comm/ros_bridge/include/TypeFactory.h

-#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.
-class TypeFactory
-{
-    typedef MessageBaseClass 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!
-        return false;
-    }
-
-    // ===========================================================================
-    // Point32Group
-    // ===========================================================================
-    template<class U>
-    bool _create(const rapidjson::Document &doc, U &type, Type2Type<MessageGroups::Point32Group>){
-        using namespace ROSBridge;
-        bool ret = JsonMethodes::GeometryMsgs::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::GeographicMsgs::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::GeometryMsgs::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::GeometryMsgs::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::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;
-    }
-
-    // ===========================================================================
-    // HeartbeatGroup
-    // ===========================================================================
-    template<class U>
-    bool _create(const rapidjson::Document &doc, U &type, Type2Type<MessageGroups::HeartbeatGroup>){
-        using namespace ROSBridge;
-        bool ret = JsonMethodes::NemoMsgs::Heartbeat::fromJson(doc, type);
-        assert(ret);
-        return ret;
-    }
-
-
-};
-
-} // end namespace ros_bridge

src/comm/ros_bridge/include/com_private.cpp

+#include "ros_bridge/include/com_private.h"
+
+#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
+
+#include <functional>
+#include <iostream>
+
+std::size_t ros_bridge::com_private::getHash(const std::string &str)
+{
+    std::hash<std::string> hash;
+    return hash(str);
+}
+
+std::size_t ros_bridge::com_private::getHash(const char *str)
+{
+    return ros_bridge::com_private::getHash(std::string(str));
+}
+
+bool ros_bridge::com_private::getTopic(const ros_bridge::com_private::JsonDoc &doc, std::string &topic)
+{
+    if ( doc.HasMember("topic") && doc["topic"].IsString() ){
+        topic = doc["topic"].GetString();
+        return true;
+    } else
+        return false;
+}
+
+bool ros_bridge::com_private::removeTopic(ros_bridge::com_private::JsonDoc &doc)
+{
+    if ( doc.HasMember("topic") && doc["topic"].IsString() ){
+        doc.RemoveMember("topic");
+        return true;
+    } else
+        return false;
+}
+
+bool ros_bridge::com_private::getType(const ros_bridge::com_private::JsonDoc &doc, std::string &type)
+{
+    if ( doc.HasMember("type") && doc["type"].IsString() ){
+        type = doc["type"].GetString();
+        return true;
+    } else
+        return false;
+}
+
+bool ros_bridge::com_private::removeType(ros_bridge::com_private::JsonDoc &doc)
+{
+    if ( doc.HasMember("type") && doc["type"].IsString() ){
+        doc.RemoveMember("type");
+        return true;
+    } else
+        return false;
+}

src/comm/ros_bridge/include/ComPrivateInclude.h → src/comm/ros_bridge/include/com_private.h

 #pragma once
 
-#include "ros_bridge/include/MessageTag.h"
 #include "ros_bridge/rapidjson/include/rapidjson/document.h"
 
 #include <memory>
 #include <deque>
 #include <unordered_map>
 
-namespace ROSBridge {
-namespace ComPrivate {
+namespace ros_bridge {
+namespace com_private {
 
-typedef MessageTag Tag;
 typedef rapidjson::Document JsonDoc;
 typedef std::unique_ptr<JsonDoc> JsonDocUPtr;
 typedef std::deque<JsonDocUPtr> JsonQueue;
@@ -20,11 +18,17 @@ using ClientMap = std::unordered_map<HashType, std::string>;
 
 static const char* _topicAdvertiserKey = "topic_advertiser";
 static const char* _topicPublisherKey =  "topic_publisher";
-//static const char* _topicAdvertiserKey = "service_advertiser";
+static const char* _serviceAdvertiserKey = "service_advertiser";
 static const char* _topicSubscriberKey = "topic_subscriber";
 
 std::size_t getHash(const std::string &str);
 std::size_t getHash(const char *str);
 
+bool getTopic(const JsonDoc &doc, std::string &topic);
+bool getType(const JsonDoc &doc, std::string &type);
+
+bool removeTopic(JsonDoc &doc);
+bool removeType(JsonDoc &doc);
+
 }
 }

src/comm/ros_bridge/include/MessageTraits.h → src/comm/ros_bridge/include/message_traits.h

 #pragma once
 
-namespace ROSBridge {
+namespace ros_bridge {
 namespace traits {
 
 
@@ -182,5 +182,19 @@ struct Select<0, T, U> {typedef U Result;};
 struct HasComponents {};
 struct Has3Components : public HasComponents {};
 struct Has2Components : public HasComponents {};
+
+
+
+template <typename T>
+struct Type2Type{
+    typedef T OriginalType;
+};
+
+
+template <int k>
+struct Int2Type{
+    enum {value = k};
+};
+
 }
 }

src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.cpp

+#include "geopoint.h"
+
+std::string ros_bridge::messages::geographic_msgs::geo_point::messageType(){
+    return "geographic_msgs/GeoPoint";
+}

src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h

+#pragma once
+
+#include <string>
+
+#include "ros_bridge/include/message_traits.h"
+#include "ros_bridge/rapidjson/include/rapidjson/document.h"
+
+namespace ros_bridge {
+//! @brief Namespace containing classes and methodes ros message generation.
+namespace messages {
+//! @brief Namespace containing classes and methodes for geometry_msgs generation.
+namespace geographic_msgs {
+//! @brief Namespace containing methodes for geometry_msgs/GeoPoint message generation.
+namespace geo_point {
+
+std::string messageType();
+
+//! @brief C++ representation of geographic_msgs/GeoPoint.
+template<class FloatType = _Float64, class OStream = std::ostream>
+class GenericGeoPoint{
+public:
+    GenericGeoPoint()
+        : _latitude(0)
+        , _longitude(0)
+        , _altitude(0)
+    {}
+    GenericGeoPoint(const GenericGeoPoint &other)
+        : _latitude(other.latitude())
+        , _longitude(other.longitude())
+        , _altitude(other.altitude())
+    {}
+    GenericGeoPoint(FloatType latitude, FloatType longitude, FloatType altitude)
+        : _latitude(latitude)
+        , _longitude(longitude)
+        , _altitude(altitude)
+    {}
+
+    FloatType latitude()     const {return _latitude;}
+    FloatType longitude()    const {return _longitude;}
+    FloatType altitude()     const {return _altitude;}
+
+    void setLatitude    (FloatType latitude)    {_latitude  = latitude;}
+    void setLongitude   (FloatType longitude)   {_longitude = longitude;}
+    void setAltitude    (FloatType altitude)    {_altitude  = altitude;}
+
+    bool operator==(const GenericGeoPoint &p1) {
+        return (   p1._latitude == this->_latitude
+                && p1._longitude == this->_longitude
+                && p1._altitude == this->_altitude);
+    }
+
+    bool operator!=(const GenericGeoPoint &p1) {
+        return !this->operator==(p1);
+    }
+
+    friend OStream& operator<<(OStream& os, const GenericGeoPoint& p)
+    {
+        os << "lat: " << p._latitude << " lon: " << p._longitude<< " alt: " << p._altitude;
+        return os;
+    }
+
+private:
+    FloatType _latitude;
+    FloatType _longitude;
+    FloatType _altitude;
+};
+typedef GenericGeoPoint<> GeoPoint;
+
+namespace detail {
+
+    template <class T>
+    auto getAltitude(const T &p, traits::Type2Type<traits::Has3Components>)
+    {
+        return p.altitude();
+    }
+
+    template <class T>
+    auto getAltitude(const T &p, traits::Type2Type<traits::Has2Components>)
+    {
+        (void)p;
+        return 0.0;
+    }
+
+    template <class T>
+    void setAltitude(const rapidjson::Value &doc, T &p, traits::Type2Type<traits::Has3Components>)
+    {
+        p.setAltitude(doc.GetFloat());
+    }
+
+    template <class T>
+    void setAltitude(const rapidjson::Value &doc, T &p, traits::Type2Type<traits::Has2Components>)
+    {
+        (void)doc;
+        (void)p;
+    }
+} // namespace detail
+
+template <class T>
+bool toJson(const T&p, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
+{
+    value.AddMember("latitude", rapidjson::Value().SetFloat((_Float64)p.latitude()), allocator);
+    value.AddMember("longitude", rapidjson::Value().SetFloat((_Float64)p.longitude()), allocator);
+
+    typedef typename traits::Select<
+            traits::HasMemberAltitude<T>::value,
+            traits::Has3Components,
+            traits::Has2Components>::Result
+            Components; // Check if PointType has 2 or 3 dimensions.
+    auto altitude = detail::getAltitude(p, traits::Type2Type<Components>()); // If T has no member altitude() replace it by 0.0;
+
+    value.AddMember("altitude", rapidjson::Value().SetFloat((_Float64)altitude), allocator);
+    return true;
+}
+
+
+
+template <class PointType>
+bool fromJson(const rapidjson::Value &value,
+              PointType &p)     {
+    if (!value.HasMember("latitude") || !value["latitude"].IsFloat()){
+        assert(false);
+        return false;
+    }
+    if (!value.HasMember("longitude") || !value["longitude"].IsFloat()){
+        assert(false);
+        return false;
+    }
+    if (!value.HasMember("altitude") || !value["altitude"].IsFloat()){
+        assert(false);
+        return false;
+    }
+
+    p.setLatitude(value["latitude"].GetFloat());
+    p.setLongitude(value["longitude"].GetFloat());
+    typedef typename traits::Select<
+            traits::HasMemberSetAltitude<PointType>::value,
+            traits::Has3Components,
+            traits::Has2Components>::Result
+            Components; // Check if PointType has 2 or 3 dimensions.
+    detail::setAltitude(value["altitude"], p,  traits::Type2Type<Components>()); // If T has no member altitude() discard doc["altitude"];
+
+    return true;
+} // namespace detail
+
+
+} // namespace geopoint
+} // namespace geometry_msgs
+} // namespace messages
+} // namespace ros_bridge

src/comm/ros_bridge/include/messages/geometry_msgs/point32.cpp

+#include "point32.h"
+
+std::string ros_bridge::messages::geometry_msgs::point32::messageType(){
+    return "geometry_msgs/Point32";
+}
+

src/comm/ros_bridge/include/messages/geometry_msgs/polygon.cpp

+#include "polygon.h"
+
+std::string ros_bridge::messages::geometry_msgs::polygon::messageType(){
+    return "geometry_msgs/Polygon";
+}
+

src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.cpp

+#include "polygon_stamped.h"
+
+std::string ros_bridge::messages::geometry_msgs::polygon_stamped::messageType(){
+    return "geometry_msgs/PolygonStamped";
+}
+

src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.cpp

+#include "polygon_array.h"
+
+std::string ros_bridge::messages::jsk_recognition_msgs::polygon_array::messageType(){
+    return "jsk_recognition_msgs/PolygonArray";
+}
+

src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.cpp

+#include "heartbeat.h"
+
+std::string ros_bridge::messages::nemo_msgs::heartbeat::messageType(){
+    return "nemo_msgs/Heartbeat";
+}
+

src/comm/ros_bridge/include/messages/nemo_msgs/progress.cpp

+#include "progress.h"
+
+std::string ros_bridge::messages::nemo_msgs::progress::messageType(){
+    return "nemo_msgs/Progress";
+}
+

src/comm/ros_bridge/include/messages/std_msgs/time.cpp

+#include "time.h"
+
+std::string ros_bridge::messages::std_msgs::time::messageType(){
+    return "std_msgs/Time";
+}