Merge branch 'improve_json' into phil

qgroundcontrol.pro

@@ -28,11 +28,11 @@ QGCROOT  = $$PWD
 DebugBuild {
     DESTDIR  = $${OUT_PWD}/debug
     DEFINES += DEBUG
-    #DEFINES += ROS_BRIDGE_CLIENT_DEBUG
+    #DEFINES += ROS_BRIDGE_DEBUG
 }
 else {
     DESTDIR  = $${OUT_PWD}/release
-    #DEFINES += DEBUG
+    #DEFINES += ROS_BRIDGE_DEBUG
     DEFINES += NDEBUG
 }
 
@@ -437,8 +437,7 @@ HEADERS += \
     src/Wima/Geometry/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 \
@@ -483,17 +482,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/Server.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/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 \
@@ -511,13 +514,20 @@ 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/RosBridgeClient.cpp \
-    src/comm/ros_bridge/include/Server.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/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 \
@@ -549,7 +559,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/Wima/Geometry/GeoPoint3D.cpp

@@ -19,11 +19,6 @@ GeoPoint3D::GeoPoint3D(const QGeoCoordinate &p, QObject *parent)
     : QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
 {}
 
-GeoPoint3D *GeoPoint3D::Clone() const
-{
-    return new GeoPoint3D(*this);
-}
-
 GeoPoint3D &GeoPoint3D::operator=(const GeoPoint3D &p)
 {
     this->setLatitude(p.latitude());

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;
+typedef ros_bridge::messages::geographic_msgs::geo_point::GeoPoint ROSGeoPoint;
 class GeoPoint3D : public QObject, public ROSGeoPoint
 {
     Q_OBJECT
 public:
-    typedef MsgGroups::GeoPointGroup Group;
 
     GeoPoint3D(QObject *parent = nullptr);
     GeoPoint3D(double latitude,
@@ -29,7 +24,6 @@ public:
     GeoPoint3D(const QGeoCoordinate& p,
                QObject *parent = nullptr);
 
-    virtual GeoPoint3D *Clone() const override;
     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(){
@@ -45,9 +37,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/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/Snake/SnakeTilesLocal.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> SnakeTilesLocal;
+typedef WimaPolygonArray<Polygon2D, QVector> SnakeTilesLocal;

src/Wima/WimaController.cc

 #include "WimaController.h"
+
 #include "utilities.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 "ros_bridge/include/CasePacker.h"
+#include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
+#include "ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h"
+#include "ros_bridge/include/messages/nemo_msgs/progress.h"
+#include "ros_bridge/include/messages/nemo_msgs/heartbeat.h"
 
-#include "Snake/QtROSJsonFactory.h"
-#include "Snake/QtROSTypeFactory.h"
 #include "Snake/QNemoProgress.h"
 #include "Snake/QNemoHeartbeat.h"
 
@@ -87,6 +89,7 @@ WimaController::WimaController(QObject *parent)
     , _batteryLevelTicker       (EVENT_TIMER_INTERVAL, 1000 /*ms*/)
     , _snakeTicker              (EVENT_TIMER_INTERVAL, 200 /*ms*/)
     , _nemoTimeoutTicker        (EVENT_TIMER_INTERVAL, 5000 /*ms*/)
+    , _topicServiceSetupDone    (false)
 {    
 
     // ROS Bridge.
@@ -94,11 +97,11 @@ WimaController::WimaController(QObject *parent)
     auto connectionStringFact       = wimaSettings->rosbridgeConnectionString();
     auto setConnectionString = [connectionStringFact, this]{
         auto connectionString = connectionStringFact->rawValue().toString();
-        if ( ROSBridge::isValidConnectionString(connectionString.toLocal8Bit().data()) ){
-            this->_pRosBridge.reset(new ROSBridge::ROSBridge(connectionString.toLocal8Bit().data()));
+        if ( ros_bridge::isValidConnectionString(connectionString.toLocal8Bit().data()) ){
+            this->_pRosBridge.reset(new ros_bridge::ROSBridge(connectionString.toLocal8Bit().data()));
         } else {
             qgcApp()->showMessage("ROS Bridge connection string invalid: " + connectionString);
-            this->_pRosBridge.reset(new ROSBridge::ROSBridge("localhost:9090"));
+            this->_pRosBridge.reset(new ros_bridge::ROSBridge("localhost:9090"));
         }
     };
     setConnectionString();
@@ -550,13 +553,28 @@ bool WimaController::setWimaPlanData(const WimaPlanData &planData)
 
     if ( _enableSnake.rawValue().toBool()
          && _pRosBridge->isRunning()
-            && _pRosBridge->connected() ){
-        if ( _snakeTilesLocal.polygons().size() > 0 ){
-            _pRosBridge->publish(_snakeOrigin, "/snake/origin");
-            _pRosBridge->publish(_snakeTilesLocal, "/snake/tiles");
-        }
+         && _pRosBridge->connected()
+         && _topicServiceSetupDone
+         && _snakeTilesLocal.polygons().size() > 0
+         )
+    {
+        using namespace ros_bridge::messages;
+        // Publish snake origin.
+        JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
+        bool ret = geographic_msgs::geo_point::toJson(
+                     _snakeOrigin, *jOrigin, jOrigin->GetAllocator());
+        Q_ASSERT(ret);
+        _pRosBridge->publish(std::move(jOrigin), "/snake/origin");
+        // Publish snake tiles.
+        JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
+        ret = jsk_recognition_msgs::polygon_array::toJson(
+                     _snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator());
+        Q_ASSERT(ret);
+        _pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
+
     }
 
+
     _localPlanDataValid = true;
     return true;
 }
@@ -747,21 +765,24 @@ void WimaController::_eventTimerHandler()
     }
 
     if ( _snakeTicker.ready() ) {
-        static bool setupDone = false;
         if ( _enableSnake.rawValue().toBool() ) {
             if ( !_pRosBridge->isRunning()) {
                 _pRosBridge->start();
-            } else if ( _pRosBridge->isRunning() && _pRosBridge->connected() && !setupDone) {
+            } else if ( _pRosBridge->isRunning()
+                        && _pRosBridge->connected()
+                        && !_topicServiceSetupDone) {
+                if ( _doTopicServiceSetup() )
+                    _topicServiceSetupDone = true;
+            } else if ( _pRosBridge->isRunning()
+                        && !_pRosBridge->connected()
+                        && _topicServiceSetupDone){
                 _pRosBridge->reset();
                 _pRosBridge->start();
-                _setupTopicService();
-                setupDone = true;
-            } else if ( _pRosBridge->isRunning() && !_pRosBridge->connected() ){
-                setupDone = false;
+                _topicServiceSetupDone = false;
             }
         } else  if ( _pRosBridge->isRunning() ) {
                 _pRosBridge->reset();
-                setupDone = false;
+                _topicServiceSetupDone = false;
         }
     }
 }
@@ -937,35 +958,74 @@ void WimaController::_switchSnakeManager(QVariant variant)
     }
 }
 
-void WimaController::_setupTopicService()
+bool WimaController::_doTopicServiceSetup()
 {
-    if ( _snakeTilesLocal.polygons().size() > 0){
-        _pRosBridge->publish(_snakeOrigin, "/snake/origin");
-        _pRosBridge->publish(_snakeTilesLocal, "/snake/tiles");
-    }
+    using namespace ros_bridge::messages;
+
+    if ( _snakeTilesLocal.polygons().size() == 0)
+        return false;
+
+    // Publish snake origin.
+    _pRosBridge->advertiseTopic("/snake/origin",
+                                geographic_msgs::geo_point::messageType().c_str());
+    JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
+    bool ret = geographic_msgs::geo_point::toJson(
+                _snakeOrigin, *jOrigin.get(), jOrigin->GetAllocator());
+    Q_ASSERT(ret);
+    (void)ret;
+    _pRosBridge->publish(std::move(jOrigin), "/snake/origin");
+
+
+    // Publish snake tiles.
+    _pRosBridge->advertiseTopic("/snake/tiles",
+                                jsk_recognition_msgs::polygon_array::messageType().c_str());
+    JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
+    ret = jsk_recognition_msgs::polygon_array::toJson(
+                _snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator());
+    Q_ASSERT(ret);
+    (void)ret;
+    _pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
+
+
+    // Subscribe nemo progress.
     _pRosBridge->subscribe("/nemo/progress",
                            /* callback */ [this](JsonDocUPtr pDoc){
         int requiredSize = this->_snakeTilesLocal.polygons().size();
-        auto& progress = this->_nemoProgress;
-        if ( !this->_pRosBridge->casePacker()->unpack(pDoc, progress)
-             || progress.progress().size() != requiredSize ) { // Some error occured.
+        auto& progress_msg = this->_nemoProgress;
+        if ( !nemo_msgs::progress::fromJson(*pDoc, progress_msg)
+             || progress_msg.progress().size() != requiredSize ) { // Some error occured.
             // Set progress to default.
-            progress.progress().fill(0, requiredSize);
-            // Publish origin and tiles again.
-            if ( this->_snakeTilesLocal.polygons().size() > 0){
-                this->_pRosBridge->publish(this->_snakeOrigin, "/snake/origin");
-                this->_pRosBridge->publish(this->_snakeTilesLocal, "/snake/tiles");
-            }
+            progress_msg.progress().fill(0, requiredSize);            
+
+            // Publish snake origin.
+            JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
+            bool ret = geographic_msgs::geo_point::toJson(
+                         this->_snakeOrigin, *jOrigin, jOrigin->GetAllocator());
+            Q_ASSERT(ret);
+            (void)ret;
+            this->_pRosBridge->publish(std::move(jOrigin), "/snake/origin");
+
+            // Publish snake tiles.
+            JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
+            ret = jsk_recognition_msgs::polygon_array::toJson(
+                         this->_snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator());
+            Q_ASSERT(ret);
+            (void)ret;
+            this->_pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
         }
 
         emit WimaController::nemoProgressChanged();
     });
+
+
+    // Subscribe /nemo/heartbeat.
     _pRosBridge->subscribe("/nemo/heartbeat",
                            /* callback */ [this](JsonDocUPtr pDoc){
-        if ( !this->_pRosBridge->casePacker()->unpack(pDoc, this->_nemoHeartbeat) ) {
-            if ( this->_nemoHeartbeat.status() == this->_fallbackStatus )
+        auto &heartbeat_msg = this->_nemoHeartbeat;
+        if ( !nemo_msgs::heartbeat::fromJson(*pDoc, heartbeat_msg) ) {
+            if ( heartbeat_msg.status() == this->_fallbackStatus )
                 return;
-            this->_nemoHeartbeat.setStatus(this->_fallbackStatus);
+            heartbeat_msg.setStatus(this->_fallbackStatus);
         }
 
         this->_nemoTimeoutTicker.reset();
@@ -974,31 +1034,38 @@ void WimaController::_setupTopicService()
         emit WimaController::nemoStatusStringChanged();
     });
 
-    _pRosBridge->advertiseService("/snake/get_origin", "snake_msgs/GetOrigin",
-                                  [this](JsonDocUPtr) -> JsonDocUPtr {
-        JsonDocUPtr pDoc;
-        if ( this->_snakeTilesLocal.polygons().size() > 0){
-            pDoc = this->_pRosBridge->casePacker()->pack(this->_snakeOrigin, "");
-        } else {
-            pDoc = this->_pRosBridge->casePacker()->pack(::GeoPoint3D(0,0,0), "");
-        }
-        this->_pRosBridge->casePacker()->removeTag(pDoc);
-        rapidjson::Document value(rapidjson::kObjectType);
-        JsonDocUPtr         pReturn(new rapidjson::Document(rapidjson::kObjectType));
-        value.CopyFrom(*pDoc, pReturn->GetAllocator());
-        pReturn->AddMember("origin", value, pReturn->GetAllocator());
 
-        return pReturn;
+    // Advertise /snake/get_origin.
+    _pRosBridge->advertiseService("/snake/get_origin", "snake_msgs/GetOrigin",
+                                  [this](JsonDocUPtr) -> JsonDocUPtr
+    {
+        using namespace ros_bridge::messages;
+
+        JsonDocUPtr pDoc(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
+        ::GeoPoint3D origin = this->_snakeOrigin;
+        rapidjson::Value jOrigin(rapidjson::kObjectType);
+        bool ret = geographic_msgs::geo_point::toJson(
+                    origin, jOrigin, pDoc->GetAllocator());
+        Q_ASSERT(ret);
+        (void)ret;
+        pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator());
+        return pDoc;
     });
 
+
+    // Advertise /snake/get_tiles.
     _pRosBridge->advertiseService("/snake/get_tiles", "snake_msgs/GetTiles",
-                                  [this](JsonDocUPtr) -> JsonDocUPtr {
-        JsonDocUPtr pDoc = this->_pRosBridge->casePacker()->pack(this->_snakeTilesLocal, "");
-        this->_pRosBridge->casePacker()->removeTag(pDoc);
-        rapidjson::Document value(rapidjson::kObjectType);
-        JsonDocUPtr         pReturn(new rapidjson::Document(rapidjson::kObjectType));
-        value.CopyFrom(*pDoc, pReturn->GetAllocator());
-        pReturn->AddMember("tiles", value, pReturn->GetAllocator());
-        return pReturn;
+                                  [this](JsonDocUPtr) -> JsonDocUPtr
+    {
+        JsonDocUPtr pDoc(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
+        rapidjson::Value jSnakeTiles(rapidjson::kObjectType);
+        bool ret = jsk_recognition_msgs::polygon_array::toJson(
+                    this->_snakeTilesLocal, jSnakeTiles, pDoc->GetAllocator());
+        Q_ASSERT(ret);
+        (void)ret;
+        pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator());
+        return pDoc;
     });
+
+    return true;
 }

src/Wima/WimaController.h

@@ -35,12 +35,13 @@
 #include "Snake/QNemoProgress.h"
 #include "Snake/QNemoHeartbeat.h"
 
-#include "ros_bridge/include/ROSBridge.h"
-#include "ros_bridge/include/CasePacker.h"
+#include "ros_bridge/include/ros_bridge.h"
 
 #include "WaypointManager/DefaultManager.h"
 #include "WaypointManager/RTLManager.h"
 
+#include "utilities.h"
+
 #include <map>
 
 
@@ -54,7 +55,7 @@ class WimaController : public QObject
 
     enum FileType {WimaFile, PlanFile};
 
-    typedef QScopedPointer<ROSBridge::ROSBridge> ROSBridgePtr;
+    typedef QScopedPointer<ros_bridge::ROSBridge> ROSBridgePtr;
 
 public:
 
@@ -336,7 +337,7 @@ private slots:
     void _snakeStoreWorkerResults            ();
     void _initStartSnakeWorker               ();
     void _switchSnakeManager                 (QVariant variant);
-    void _setupTopicService                     ();
+    bool _doTopicServiceSetup();
     // Periodic tasks.
     void _eventTimerHandler                  (void);
     // Waypoint manager handling.
@@ -344,7 +345,6 @@ private slots:
 
 private:
     using StatusMap = std::map<int, QString>;
-    using CasePacker = ROSBridge::CasePacker;
 
     // Controllers.
     PlanMasterController   *_masterController;
@@ -407,6 +407,7 @@ private:
     int                     _fallbackStatus;
     ROSBridgePtr            _pRosBridge;
     static StatusMap        _nemoStatusMap;
+    bool                    _topicServiceSetupDone;
 
     // Periodic tasks.
     QTimer      _eventTimer;

src/Wima/WimaController_new.cc

-#include "WimaController.h"
-#include "utilities.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 "ros_bridge/include/CasePacker.h"
-
-#include "Snake/QtROSJsonFactory.h"
-#include "Snake/QtROSTypeFactory.h"
-#include "Snake/QNemoProgress.h"
-#include "Snake/QNemoHeartbeat.h"
-
-#include "QVector3D"
-#include <QScopedPointer>
-
-
-
-// const char* WimaController::wimaFileExtension           = "wima";
-const char* WimaController::areaItemsName               = "AreaItems";
-const char* WimaController::missionItemsName            = "MissionItems";
-const char* WimaController::settingsGroup               = "WimaController";
-const char* WimaController::enableWimaControllerName    = "EnableWimaController";
-const char* WimaController::overlapWaypointsName        = "OverlapWaypoints";
-const char* WimaController::maxWaypointsPerPhaseName    = "MaxWaypointsPerPhase";
-const char* WimaController::startWaypointIndexName      = "StartWaypointIndex";
-const char* WimaController::showAllMissionItemsName     = "ShowAllMissionItems";
-const char* WimaController::showCurrentMissionItemsName = "ShowCurrentMissionItems";
-const char* WimaController::flightSpeedName             = "FlightSpeed";
-const char* WimaController::arrivalReturnSpeedName      = "ArrivalReturnSpeed";
-const char* WimaController::altitudeName                = "Altitude";
-const char* WimaController::snakeTileWidthName          = "SnakeTileWidth";
-const char* WimaController::snakeTileHeightName         = "SnakeTileHeight";
-const char* WimaController::snakeMinTileAreaName        = "SnakeMinTileArea";
-const char* WimaController::snakeLineDistanceName       = "SnakeLineDistance";
-const char* WimaController::snakeMinTransectLengthName  = "SnakeMinTransectLength";
-
-WimaController::StatusMap WimaController::_nemoStatusMap{
-    std::make_pair<int, QString>(0, "No Heartbeat"),
-    std::make_pair<int, QString>(1, "Connected"),
-    std::make_pair<int, QString>(-1, "Timeout")};
-
-using namespace snake;
-using namespace snake_geometry;
-
-WimaController::WimaController(QObject *parent)
-    : QObject                   (parent)
-    , _joinedArea               ()
-    , _measurementArea          ()
-    , _serviceArea              ()
-    , _corridor                 ()
-    , _localPlanDataValid       (false)
-    , _areaInterface            (&_measurementArea, &_serviceArea, &_corridor, &_joinedArea)
-    , _managerSettings          ()
-    , _defaultManager           (_managerSettings, _areaInterface)
-    , _snakeManager             (_managerSettings, _areaInterface)
-    , _rtlManager               (_managerSettings, _areaInterface)
-    , _currentManager           (&_defaultManager)
-    , _managerList              {&_defaultManager, &_snakeManager, &_rtlManager}
-    , _metaDataMap              (FactMetaData::createMapFromJsonFile(
-                                     QStringLiteral(":/json/WimaController.SettingsGroup.json"), this))
-    , _enableWimaController     (settingsGroup, _metaDataMap[enableWimaControllerName])
-    , _overlapWaypoints         (settingsGroup, _metaDataMap[overlapWaypointsName])
-    , _maxWaypointsPerPhase     (settingsGroup, _metaDataMap[maxWaypointsPerPhaseName])
-    , _nextPhaseStartWaypointIndex       (settingsGroup, _metaDataMap[startWaypointIndexName])
-    , _showAllMissionItems      (settingsGroup, _metaDataMap[showAllMissionItemsName])
-    , _showCurrentMissionItems  (settingsGroup, _metaDataMap[showCurrentMissionItemsName])    
-    , _flightSpeed              (settingsGroup, _metaDataMap[flightSpeedName])
-    , _arrivalReturnSpeed       (settingsGroup, _metaDataMap[arrivalReturnSpeedName])
-    , _altitude                 (settingsGroup, _metaDataMap[altitudeName])
-    , _measurementPathLength    (-1)
-    , _lowBatteryHandlingTriggered  (false)
-    , _snakeCalcInProgress      (false)
-    , _snakeTileWidth           (settingsGroup, _metaDataMap[snakeTileWidthName])
-    , _snakeTileHeight          (settingsGroup, _metaDataMap[snakeTileHeightName])
-    , _snakeMinTileArea         (settingsGroup, _metaDataMap[snakeMinTileAreaName])
-    , _snakeLineDistance        (settingsGroup, _metaDataMap[snakeLineDistanceName])
-    , _snakeMinTransectLength   (settingsGroup, _metaDataMap[snakeMinTransectLengthName])
-    , _nemoHeartbeat            (0 /*status: not connected*/)
-    , _fallbackStatus           (0 /*status: not connected*/)
-    , _bridgeSetupDone          (false)
-    , _pRosBridge               (new ROSBridge::ROSBridge())
-{
-    // Set up facts.
-    _showAllMissionItems.setRawValue(true);
-    _showCurrentMissionItems.setRawValue(true);
-    connect(&_overlapWaypoints,             &Fact::rawValueChanged, this, &WimaController::_updateOverlap);
-    connect(&_maxWaypointsPerPhase,         &Fact::rawValueChanged, this, &WimaController::_updateMaxWaypoints);
-    connect(&_nextPhaseStartWaypointIndex,  &Fact::rawValueChanged, this, &WimaController::_setStartIndex);
-    connect(&_flightSpeed,                  &Fact::rawValueChanged, this, &WimaController::_updateflightSpeed);
-    connect(&_arrivalReturnSpeed,           &Fact::rawValueChanged, this, &WimaController::_updateArrivalReturnSpeed);
-    connect(&_altitude,                     &Fact::rawValueChanged, this, &WimaController::_updateAltitude);
-
-    // Init waypoint managers.
-    bool value;
-    size_t overlap  = _overlapWaypoints.rawValue().toUInt(&value);
-    Q_ASSERT(value);
-    size_t N        = _maxWaypointsPerPhase.rawValue().toUInt(&value);
-    Q_ASSERT(value);
-    size_t startIdx = _nextPhaseStartWaypointIndex.rawValue().toUInt(&value);
-    Q_ASSERT(value);
-    (void)value;
-    for (auto manager : _managerList){
-        manager->setOverlap(overlap);
-        manager->setN(N);
-        manager->setStartIndex(startIdx);
-    }
-
-    // Periodic.
-    connect(&_eventTimer, &QTimer::timeout, this, &WimaController::_eventTimerHandler);
-    //_eventTimer.setInterval(EVENT_TIMER_INTERVAL);
-    _eventTimer.start(EVENT_TIMER_INTERVAL);
-
-    // Snake Worker Thread.
-    connect(&_snakeWorker, &SnakeDataManager::finished, this, &WimaController::_updateSnakeData);
-    connect(this, &WimaController::nemoProgressChanged, this, &WimaController::_initStartSnakeWorker);
-    connect(this, &QObject::destroyed, &this->_snakeWorker, &SnakeDataManager::quit);
-
-    // Snake.
-    connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_switchSnakeManager);
-    _switchSnakeManager(_enableSnake.rawValue());
-}
-
-PlanMasterController *WimaController::masterController() {
-    return _masterController;
-}
-
-MissionController *WimaController::missionController() {
-    return _missionController;
-}
-
-QmlObjectListModel *WimaController::visualItems() {
-    return &_areas;
-}
-
-QmlObjectListModel *WimaController::missionItems() {
-    return const_cast<QmlObjectListModel*>(&_currentManager->missionItems());
-}
-
-QmlObjectListModel *WimaController::currentMissionItems() {
-    return const_cast<QmlObjectListModel*>(&_currentManager->currentMissionItems());
-}
-
-QVariantList WimaController::waypointPath()
-{
-    return const_cast<QVariantList&>(_currentManager->waypointsVariant());
-}
-
-QVariantList WimaController::currentWaypointPath()
-{
-    return const_cast<QVariantList&>(_currentManager->currentWaypointsVariant());
-}
-
-Fact *WimaController::enableWimaController() {
-    return &_enableWimaController;
-}
-
-Fact *WimaController::overlapWaypoints() {
-    return &_overlapWaypoints;
-}
-
-Fact *WimaController::maxWaypointsPerPhase() {
-    return &_maxWaypointsPerPhase;
-}
-
-Fact *WimaController::startWaypointIndex() {
-    return &_nextPhaseStartWaypointIndex;
-}
-
-Fact *WimaController::showAllMissionItems() {
-    return &_showAllMissionItems;
-}
-
-Fact *WimaController::showCurrentMissionItems() {
-    return &_showCurrentMissionItems;
-}
-
-Fact *WimaController::flightSpeed() {
-    return &_flightSpeed;
-}
-
-Fact *WimaController::arrivalReturnSpeed() {
-    return &_arrivalReturnSpeed;
-}
-
-Fact *WimaController::altitude() {
-    return &_altitude;
-}
-
-QVector<int> WimaController::nemoProgress() {
-    if ( _nemoProgress.progress().size() == _snakeTileCenterPoints.size() )
-        return _nemoProgress.progress();
-    else
-        return QVector<int>(_snakeTileCenterPoints.size(), 0);
-}
-
-double WimaController::phaseDistance() const
-{
-    return 0.0;
-}
-
-double WimaController::phaseDuration() const
-{
-    return 0.0;
-}
-
-int WimaController::nemoStatus() const
-{
-    return _nemoHeartbeat.status();
-}
-
-QString WimaController::nemoStatusString() const
-{
-    return _nemoStatusMap.at(_nemoHeartbeat.status());
-}
-
-bool WimaController::snakeCalcInProgress() const
-{
-    return _snakeCalcInProgress;
-}
-
-void WimaController::setMasterController(PlanMasterController *masterC)
-{
-    _masterController = masterC;
-    _managerSettings.setMasterController(masterC);
-    emit masterControllerChanged();
-}
-
-void WimaController::setMissionController(MissionController *missionC)
-{
-    _missionController = missionC;
-    _managerSettings.setMissionController(missionC);
-    emit missionControllerChanged();
-}
-
-void WimaController::nextPhase()
-{
-    _calcNextPhase();
-}
-
-void WimaController::previousPhase()
-{        
-    if ( !_currentManager->previous() ) {
-        Q_ASSERT(false);
-    }
-
-    emit missionItemsChanged();
-    emit currentMissionItemsChanged();
-    emit currentWaypointPathChanged();
-    emit waypointPathChanged();
-}
-
-void WimaController::resetPhase()
-{
-    if ( !_currentManager->reset() ) {
-        Q_ASSERT(false);
-    }
-
-    emit missionItemsChanged();
-    emit currentMissionItemsChanged();
-    emit currentWaypointPathChanged();
-    emit waypointPathChanged();
-}
-
-void WimaController::requestSmartRTL()
-{
-    QString errorString("Smart RTL requested. ");
-    if ( !_checkSmartRTLPreCondition(errorString) ){
-        qgcApp()->showMessage(errorString);
-        return;
-    }
-    emit smartRTLRequestConfirm();
-}
-
-bool WimaController::upload()
-{
-    auto &currentMissionItems = _defaultManager.currentMissionItems();
-    if (   !_serviceArea.containsCoordinate(_masterController->managerVehicle()->coordinate())
-        && currentMissionItems.count() > 0) {
-        emit forceUploadConfirm();
-        return false;
-    }
-
-    return forceUpload();
-}
-
-bool WimaController::forceUpload()
-{
-    auto &currentMissionItems = _defaultManager.currentMissionItems();
-    if (currentMissionItems.count() < 1)
-        return false;
-
-    _missionController->removeAll();
-    // Set homeposition of settingsItem.
-    QmlObjectListModel* visuals = _missionController->visualItems();
-    MissionSettingsItem* settingsItem = visuals->value<MissionSettingsItem *>(0);
-    if (settingsItem == nullptr) {
-        Q_ASSERT(false);
-        qWarning("WimaController::updateCurrentMissionItems(): nullptr");
-        return false;
-    }
-    settingsItem->setCoordinate(_managerSettings.homePosition());
-
-    // Copy mission items to _missionController.
-    for (int i = 1; i < currentMissionItems.count(); i++){
-        auto *item = currentMissionItems.value<const SimpleMissionItem *>(i);
-        _missionController->insertSimpleMissionItem(*item, visuals->count());
-    }
-
-    _masterController->sendToVehicle();
-
-    return true;
-}
-
-void WimaController::removeFromVehicle()
-{
-    _masterController->removeAllFromVehicle();
-    _missionController->removeAll();
-}
-
-void WimaController::executeSmartRTL()
-{
-    forceUpload();
-    masterController()->managerVehicle()->startMission();
-}
-
-void WimaController::initSmartRTL()
-{
-    _initSmartRTL();
-}
-
-void WimaController::removeVehicleTrajectoryHistory()
-{
-    Vehicle *managerVehicle = masterController()->managerVehicle();
-    managerVehicle->trajectoryPoints()->clear();
-}
-
-bool WimaController::_calcShortestPath(const QGeoCoordinate &start,
-                                       const QGeoCoordinate &destination,
-                                       QVector<QGeoCoordinate> &path)
-{
-    using namespace GeoUtilities;
-    using namespace PolygonCalculus;
-    QPolygonF polygon2D;
-    toCartesianList(_joinedArea.coordinateList(), /*origin*/ start, polygon2D);
-    QPointF start2D(0,0);
-    QPointF end2D;
-    toCartesian(destination, start, end2D);
-    QVector<QPointF> path2D;
-
-    bool retVal = PolygonCalculus::shortestPath(polygon2D, start2D, end2D, path2D);
-    toGeoList(path2D, /*origin*/ start, path);
-
-    return  retVal;
-}
-
-bool WimaController::setWimaPlanData(const WimaPlanData &planData)
-{
-    // reset visual items
-    _areas.clear();
-    _defaultManager.clear();
-    _snakeTiles.polygons().clear();
-    _snakeTilesLocal.polygons().clear();
-    _snakeTileCenterPoints.clear();
-
-    emit visualItemsChanged();
-    emit missionItemsChanged();
-    emit currentMissionItemsChanged();
-    emit waypointPathChanged();
-    emit currentWaypointPathChanged();
-    emit snakeTilesChanged();
-    emit snakeTileCenterPointsChanged();
-
-    _localPlanDataValid = false;
-
-    // extract list with WimaAreas
-    QList<const WimaAreaData*> areaList = planData.areaList();
-
-    int areaCounter = 0;
-    const int numAreas = 4; // extract only numAreas Areas, if there are more they are invalid and ignored
-    for (int i = 0; i < areaList.size(); i++) {
-        const WimaAreaData *areaData = areaList[i];
-
-        if (areaData->type() == WimaServiceAreaData::typeString) { // is it a service area?
-            _serviceArea = *qobject_cast<const WimaServiceAreaData*>(areaData);
-            areaCounter++;
-            _areas.append(&_serviceArea);
-
-            continue;
-        }
-
-        if (areaData->type() == WimaMeasurementAreaData::typeString) { // is it a measurement area?
-            _measurementArea =  *qobject_cast<const WimaMeasurementAreaData*>(areaData);
-            areaCounter++;
-            _areas.append(&_measurementArea);
-
-            continue;
-        }
-
-        if (areaData->type() == WimaCorridorData::typeString) { // is it a corridor?
-            _corridor =  *qobject_cast<const WimaCorridorData*>(areaData);
-            areaCounter++;
-            //_visualItems.append(&_corridor); // not needed
-
-            continue;
-        }
-
-        if (areaData->type() == WimaJoinedAreaData::typeString) { // is it a corridor?
-            _joinedArea =  *qobject_cast<const WimaJoinedAreaData*>(areaData);
-            areaCounter++;
-            _areas.append(&_joinedArea);
-
-            continue;
-        }
-
-        if (areaCounter >= numAreas)
-            break;
-    }
-
-    if (areaCounter != numAreas) {
-        Q_ASSERT(false);
-        return false;
-    }
-
-    emit visualItemsChanged();
-
-    // extract mission items
-    QList<MissionItem> tempMissionItems = planData.missionItems();
-    if (tempMissionItems.size() < 1) {
-        qWarning("WimaController: Mission items from WimaPlaner empty!");
-        return false;
-    }
-
-    for (auto item : tempMissionItems) {
-        _defaultManager.push_back(item.coordinate());
-    }
-
-    _managerSettings.setHomePosition( QGeoCoordinate(_serviceArea.center().latitude(),
-                                                     _serviceArea.center().longitude(),
-                                                     0) );
-
-    if( !_defaultManager.reset() ){
-        Q_ASSERT(false);
-        return false;
-    }
-
-    emit missionItemsChanged();
-    emit currentMissionItemsChanged();
-    emit waypointPathChanged();
-    emit currentWaypointPathChanged();
-
-    _localPlanDataValid = true;
-
-    _initStartSnakeWorker();
-
-    return true;
-}
-
-WimaController *WimaController::thisPointer()
-{
-    return this;
-}
-
-bool WimaController::_calcNextPhase()
-{
-    if ( !_currentManager->next() ) {
-        Q_ASSERT(false);
-        return false;
-    }
-
-    emit missionItemsChanged();
-    emit currentMissionItemsChanged();
-    emit currentWaypointPathChanged();
-    emit waypointPathChanged();
-
-    return true;
-}
-
-bool WimaController::_setStartIndex()
-{
-    bool value;
-    _currentManager->setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt(&value)-1);
-    Q_ASSERT(value);
-    (void)value;
-
-    if ( !_currentManager->update() ) {
-        Q_ASSERT(false);
-        return false;
-    }
-
-    emit missionItemsChanged();
-    emit currentMissionItemsChanged();
-    emit currentWaypointPathChanged();
-    emit waypointPathChanged();
-
-    return true;
-}
-
-void WimaController::_recalcCurrentPhase()
-{
-    if ( !_currentManager->update() ) {
-        Q_ASSERT(false);
-    }
-
-    emit missionItemsChanged();
-    emit currentMissionItemsChanged();
-    emit currentWaypointPathChanged();
-    emit waypointPathChanged();
-}
-
-void WimaController::_updateOverlap()
-{
-    bool value;
-    _currentManager->setOverlap(_overlapWaypoints.rawValue().toUInt(&value));
-    Q_ASSERT(value);
-    (void)value;
-
-    if ( !_currentManager->update() ) {
-        assert(false);
-    }
-
-    emit missionItemsChanged();
-    emit currentMissionItemsChanged();
-    emit currentWaypointPathChanged();
-    emit waypointPathChanged();
-}
-
-void WimaController::_updateMaxWaypoints()
-{
-    bool value;
-    _currentManager->setN(_maxWaypointsPerPhase.rawValue().toUInt(&value));
-    Q_ASSERT(value);
-    (void)value;
-
-    if ( !_currentManager->update() ) {
-        Q_ASSERT(false);
-    }
-
-    emit missionItemsChanged();
-    emit currentMissionItemsChanged();
-    emit currentWaypointPathChanged();
-    emit waypointPathChanged();
-}
-
-void WimaController::_updateflightSpeed()
-{
-    bool value;
-    _managerSettings.setFlightSpeed(_flightSpeed.rawValue().toDouble(&value));
-    Q_ASSERT(value);
-    (void)value;
-
-    if ( !_currentManager->update() ) {
-        Q_ASSERT(false);
-    }
-
-    emit missionItemsChanged();
-    emit currentMissionItemsChanged();
-    emit currentWaypointPathChanged();
-    emit waypointPathChanged();
-}
-
-void WimaController::_updateArrivalReturnSpeed()
-{
-    bool value;
-    _managerSettings.setArrivalReturnSpeed(_arrivalReturnSpeed.rawValue().toDouble(&value));
-    Q_ASSERT(value);
-    (void)value;
-
-    if ( !_currentManager->update() ) {
-        Q_ASSERT(false);
-    }
-
-    emit missionItemsChanged();
-    emit currentMissionItemsChanged();
-    emit currentWaypointPathChanged();
-    emit waypointPathChanged();
-}
-
-void WimaController::_updateAltitude()
-{
-    bool value;
-    _managerSettings.setAltitude(_altitude.rawValue().toDouble(&value));
-    Q_ASSERT(value);
-    (void)value;
-
-    if ( !_currentManager->update() ) {
-        Q_ASSERT(false);
-    }
-
-    emit missionItemsChanged();
-    emit currentMissionItemsChanged();
-    emit currentWaypointPathChanged();
-    emit waypointPathChanged();
-}
-
-void WimaController::_checkBatteryLevel()
-{
-    Vehicle *managerVehicle     = masterController()->managerVehicle();
-    WimaSettings* wimaSettings  = qgcApp()->toolbox()->settingsManager()->wimaSettings();
-    int batteryThreshold        = wimaSettings->lowBatteryThreshold()->rawValue().toInt();
-    bool enabled                = _enableWimaController.rawValue().toBool();
-    unsigned int minTime        = wimaSettings->minimalRemainingMissionTime()->rawValue().toUInt();
-
-    if (managerVehicle != nullptr && enabled == true) {
-        Fact *battery1percentRemaining = managerVehicle->battery1FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName);
-        Fact *battery2percentRemaining = managerVehicle->battery2FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName);
-
-
-        if (battery1percentRemaining->rawValue().toDouble() < batteryThreshold
-                && battery2percentRemaining->rawValue().toDouble() < batteryThreshold) {
-            if (!_lowBatteryHandlingTriggered) {                
-                _lowBatteryHandlingTriggered = true;
-                if ( !(_missionController->remainingTime() <= minTime) ) {
-                    requestSmartRTL();
-                }
-            }
-        }
-        else {
-            _lowBatteryHandlingTriggered = false;
-        }
-
-    }
-}
-
-void WimaController::_eventTimerHandler()
-{
-    static EventTicker batteryLevelTicker(EVENT_TIMER_INTERVAL, CHECK_BATTERY_INTERVAL);
-    static EventTicker snakeTicker(EVENT_TIMER_INTERVAL, SNAKE_EVENT_LOOP_INTERVAL);
-    static EventTicker nemoStatusTicker(EVENT_TIMER_INTERVAL, 5000);
-
-    // Battery level check necessary?
-    Fact *enableLowBatteryHandling = qgcApp()->toolbox()->settingsManager()->wimaSettings()->enableLowBatteryHandling();
-    if ( enableLowBatteryHandling->rawValue().toBool() && batteryLevelTicker.ready() )
-        _checkBatteryLevel();
-
-    // Snake flight plan update necessary?
-//    if ( snakeEventLoopTicker.ready() ) {
-//        if ( _enableSnake.rawValue().toBool() && _localPlanDataValid && !_snakeCalcInProgress && _scenarioDefinedBool) {
-//        }
-//    }
-
-    if ( nemoStatusTicker.ready() ) {
-        this->_nemoHeartbeat.setStatus(_fallbackStatus);
-        emit WimaController::nemoStatusChanged();
-        emit WimaController::nemoStatusStringChanged();
-    }
-
-    if ( snakeTicker.ready() ) {
-        if ( _enableSnake.rawValue().toBool()
-             && _pRosBridge->connected() ) {
-            if ( !_bridgeSetupDone ) {
-                qWarning() << "ROS Bridge setup. ";
-                auto start = std::chrono::high_resolution_clock::now();
-                _pRosBridge->start();
-                auto end = std::chrono::high_resolution_clock::now();
-                qWarning() << "_pRosBridge->start() time: "
-                           << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
-                           << " ms";
-                start = std::chrono::high_resolution_clock::now();
-                _pRosBridge->publish(_snakeOrigin, "/snake/origin");
-                end = std::chrono::high_resolution_clock::now();
-                qWarning() << "_pRosBridge->publish(_snakeOrigin, \"/snake/origin\") time: "
-                           << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
-                           << " ms";
-                start = std::chrono::high_resolution_clock::now();
-                _pRosBridge->publish(_snakeTilesLocal, "/snake/tiles");
-                end = std::chrono::high_resolution_clock::now();
-                qWarning() << "_pRosBridge->publish(_snakeTilesLocal, \"/snake/tiles\") time: "
-                           << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
-                           << " ms";
-
-
-                start = std::chrono::high_resolution_clock::now();
-                _pRosBridge->subscribe("/nemo/progress",
-                                       /* callback */ [this](JsonDocUPtr pDoc){
-                    int requiredSize = this->_snakeTilesLocal.polygons().size();
-                    auto& progress = this->_nemoProgress;
-                    if ( !this->_pRosBridge->casePacker()->unpack(pDoc, progress)
-                         || progress.progress().size() != requiredSize ) {
-                        progress.progress().fill(0, requiredSize);
-                    }
-
-                    emit WimaController::nemoProgressChanged();
-                });
-                end = std::chrono::high_resolution_clock::now();
-                qWarning() << "_pRosBridge->subscribe(\"/nemo/progress\" time: "
-                           << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
-                           << " ms";
-
-                start = std::chrono::high_resolution_clock::now();
-                _pRosBridge->subscribe("/nemo/heartbeat",
-                                       /* callback */ [this, &nemoStatusTicker](JsonDocUPtr pDoc){
-                    if ( !this->_pRosBridge->casePacker()->unpack(pDoc, this->_nemoHeartbeat) ) {
-                        if ( this->_nemoHeartbeat.status() == this->_fallbackStatus )
-                            return;
-                        this->_nemoHeartbeat.setStatus(this->_fallbackStatus);
-                    }
-
-                    nemoStatusTicker.reset();
-                    this->_fallbackStatus = -1; /*Timeout*/
-                    emit WimaController::nemoStatusChanged();
-                    emit WimaController::nemoStatusStringChanged();
-                });
-                end = std::chrono::high_resolution_clock::now();
-                qWarning() << "_pRosBridge->subscribe(\"/nemo/heartbeat\" time: "
-                           << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
-                           << " ms";
-                _bridgeSetupDone = true;
-            }
-        } else if ( _bridgeSetupDone) {
-            _pRosBridge->reset();
-            _bridgeSetupDone = false;
-        }
-    }
-}
-
-void WimaController::_smartRTLCleanUp(bool flying)
-{
-    if ( !flying) { // vehicle has landed
-        _switchWaypointManager(_defaultManager);
-        _missionController->removeAllFromVehicle();
-        _missionController->removeAll();
-        disconnect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp);
-    }
-}
-
-void WimaController::_setPhaseDistance(double distance)
-{
-    (void)distance;
-//    if (!qFuzzyCompare(distance, _phaseDistance)) {
-//        _phaseDistance = distance;
-
-//        emit phaseDistanceChanged();
-//    }
-}
-
-void WimaController::_setPhaseDuration(double duration)
-{
-    (void)duration;
-//    if (!qFuzzyCompare(duration, _phaseDuration)) {
-//        _phaseDuration = duration;
-
-//        emit phaseDurationChanged();
-//    }
-}
-
-bool WimaController::_checkSmartRTLPreCondition(QString &errorString)
-{
-    if (!_localPlanDataValid) {
-        errorString.append(tr("No WiMA data available. Please define at least a measurement and a service area."));
-        return false;
-    }
-
-    return _rtlManager.checkPrecondition(errorString);
-}
-
-void WimaController::_switchWaypointManager(WaypointManager::ManagerBase &manager)
-{
-    if (_currentManager != &manager) {
-        _currentManager = &manager;
-
-        disconnect(&_overlapWaypoints,             &Fact::rawValueChanged,
-                   this, &WimaController::_updateOverlap);
-        disconnect(&_maxWaypointsPerPhase,         &Fact::rawValueChanged,
-                   this, &WimaController::_updateMaxWaypoints);
-        disconnect(&_nextPhaseStartWaypointIndex,  &Fact::rawValueChanged,
-                   this, &WimaController::_setStartIndex);
-
-        _maxWaypointsPerPhase.setRawValue(_currentManager->N());
-        _overlapWaypoints.setRawValue(_currentManager->overlap());
-        _nextPhaseStartWaypointIndex.setRawValue(_currentManager->startIndex());
-
-        connect(&_overlapWaypoints,             &Fact::rawValueChanged,
-                this, &WimaController::_updateOverlap);
-        connect(&_maxWaypointsPerPhase,         &Fact::rawValueChanged,
-                this, &WimaController::_updateMaxWaypoints);
-        connect(&_nextPhaseStartWaypointIndex,  &Fact::rawValueChanged,
-                this, &WimaController::_setStartIndex);
-
-        emit missionItemsChanged();
-        emit currentMissionItemsChanged();
-        emit waypointPathChanged();
-        emit currentWaypointPathChanged();
-
-        qWarning() << "WimaController::_switchWaypointManager: statistics update missing.";
-    }
-}
-
-void WimaController::_initSmartRTL()
-{
-    QString errorString;
-    static int attemptCounter = 0;
-    attemptCounter++;
-
-    if ( _checkSmartRTLPreCondition(errorString) ) {
-        _masterController->managerVehicle()->pauseVehicle();
-        connect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::_smartRTLCleanUp);
-        if ( _rtlManager.update() ) { // Calculate return path.
-            _switchWaypointManager(_rtlManager);
-            attemptCounter = 0;
-            emit smartRTLPathConfirm();
-            return;
-        }
-    } else if (attemptCounter > SMART_RTL_MAX_ATTEMPTS) {
-        errorString.append(tr("Smart RTL: No success after maximum number of attempts."));
-        qgcApp()->showMessage(errorString);
-        attemptCounter = 0;
-    } else {
-        _smartRTLTimer.singleShot(SMART_RTL_ATTEMPT_INTERVAL, this, &WimaController::_initSmartRTL);
-    }
-}
-
-void WimaController::_setSnakeCalcInProgress(bool inProgress)
-{
-    if (_snakeCalcInProgress != inProgress){
-        _snakeCalcInProgress = inProgress;
-        emit snakeCalcInProgressChanged();
-    }
-}
-
-void WimaController::_updateSnakeData()
-{    
-    _setSnakeCalcInProgress(false);
-    auto start = std::chrono::high_resolution_clock::now();
-    _snakeManager.clear();
-    const auto& r = _snakeWorker.result();
-    if (!r.success) {
-        //qgcApp()->showMessage(r.errorMessage);
-        return;
-    }
-
-    // create Mission items from r.waypoints
-    long n = r.waypoints.size() - r.returnPathIdx.size() - r.arrivalPathIdx.size() + 2;
-    if (n < 1) {
-        return;
-    }
-
-    // Create QVector<QGeoCoordinate> containing all waypoints;
-    unsigned long startIdx = r.arrivalPathIdx.last();
-    unsigned  long endIdx = r.returnPathIdx.first();
-    for (unsigned long i = startIdx; i <= endIdx; ++i) {
-        _snakeManager.push_back(r.waypoints[int(i)]);
-    }    
-    auto end = std::chrono::high_resolution_clock::now();
-    double duration = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
-    qWarning() << "WimaController: push_back waypoints execution time: " << duration << " ms.";
-
-
-    start = std::chrono::high_resolution_clock::now();
-    _snakeManager.update();
-    end         = std::chrono::high_resolution_clock::now();
-    duration    = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
-    qWarning() << "WimaController: _snakeManager.update(); execution time: " << duration << " ms.";
-
-
-    start = std::chrono::high_resolution_clock::now();
-    emit missionItemsChanged();
-    emit currentMissionItemsChanged();
-    emit currentWaypointPathChanged();
-    emit waypointPathChanged();
-    end         = std::chrono::high_resolution_clock::now();
-    duration    = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
-    qWarning() << "WimaController: gui update execution time: " << duration << " ms.";
-
-    start = std::chrono::high_resolution_clock::now();
-    _snakeOrigin            = r.origin;
-    _snakeTileCenterPoints  = r.tileCenterPoints;
-    _snakeTiles             = r.tiles;
-    _snakeTilesLocal        = r.tilesLocal;
-    end         = std::chrono::high_resolution_clock::now();
-    duration    = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
-    qWarning() << "WimaController: tiles copy execution time: " << duration << " ms.";
-
-    start = std::chrono::high_resolution_clock::now();
-    emit snakeTilesChanged();
-    emit snakeTileCenterPointsChanged();
-    end         = std::chrono::high_resolution_clock::now();
-    duration    = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
-    qWarning() << "WimaController: gui update 2 execution time: " << duration << " ms.";
-}
-
-void WimaController::_initStartSnakeWorker()
-{
-    if ( !_enableSnake.rawValue().toBool() )
-        return;
-
-    // Stop worker thread if running.
-    if ( _snakeWorker.isRunning() ) {
-        _snakeWorker.quit();
-    }
-
-    // Initialize _snakeWorker.
-    _snakeWorker.setMeasurementArea(
-                _measurementArea.coordinateList());
-    _snakeWorker.setServiceArea(
-                _serviceArea.coordinateList());
-    _snakeWorker.setCorridor(
-                _corridor.coordinateList());
-    _snakeWorker.setProgress(
-                _nemoProgress.progress());
-    _snakeWorker.setLineDistance(
-                _snakeLineDistance.rawValue().toDouble());
-    _snakeWorker.setMinTransectLength(
-                _snakeMinTransectLength.rawValue().toDouble());
-    _snakeWorker.setTileHeight(
-                _snakeTileHeight.rawValue().toDouble());
-    _snakeWorker.setTileWidth(
-                _snakeTileWidth.rawValue().toDouble());
-    _snakeWorker.setMinTileArea(
-                _snakeMinTileArea.rawValue().toDouble());
-    _setSnakeCalcInProgress(true);
-
-    // Start worker thread.
-    _snakeWorker.start();
-}
-
-void WimaController::_switchSnakeManager(QVariant variant)
-{
-    if (variant.value<bool>()){
-        _switchWaypointManager(_snakeManager);
-    } else {
-        _switchWaypointManager(_defaultManager);
-    }
-}

src/Wima/WimaController_new.h

-#pragma once
-
-#include <QObject>
-#include <QScopedPointer>
-
-#include "QGCMapPolygon.h"
-#include "QmlObjectListModel.h"
-
-#include "Geometry/WimaArea.h"
-#include "Geometry/WimaMeasurementArea.h"
-#include "Geometry/WimaServiceArea.h"
-#include "Geometry/WimaCorridor.h"
-#include "Geometry/WimaMeasurementAreaData.h"
-#include "Geometry/WimaCorridorData.h"
-#include "Geometry/WimaServiceAreaData.h"
-
-#include "WimaPlanData.h"
-
-#include "PlanMasterController.h"
-#include "MissionController.h"
-#include "SurveyComplexItem.h"
-#include "SimpleMissionItem.h"
-#include "MissionSettingsItem.h"
-#include "JsonHelper.h"
-#include "QGCApplication.h"
-#include "SettingsFact.h"
-#include "WimaSettings.h"
-#include "SettingsManager.h"
-
-#include "snake.h"
-#include "Snake/SnakeWorker.h"
-#include "Snake/GeoPolygonArray.h"
-#include "Snake/PolygonArray.h"
-#include "Geometry/GeoPoint3D.h"
-#include "Snake/QNemoProgress.h"
-#include "Snake/QNemoHeartbeat.h"
-
-#include "ros_bridge/include/ROSBridge.h"
-
-#include "WaypointManager/DefaultManager.h"
-#include "WaypointManager/RTLManager.h"
-
-#include <map>
-
-#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
-
-
-using namespace snake;
-
-typedef std::unique_ptr<rapidjson::Document> JsonDocUPtr;
-
-class WimaController : public QObject
-{
-    Q_OBJECT
-
-    enum FileType {WimaFile, PlanFile};
-
-    typedef QScopedPointer<ROSBridge::ROSBridge> ROSBridgePtr;
-
-public:
-
-
-    WimaController(QObject *parent = nullptr);
-
-    // Controllers.
-    Q_PROPERTY(PlanMasterController*    masterController
-               READ                     masterController
-               WRITE                    setMasterController
-               NOTIFY                   masterControllerChanged
-               )
-    Q_PROPERTY(MissionController*       missionController
-               READ                     missionController
-               WRITE                    setMissionController
-               NOTIFY                   missionControllerChanged
-               )
-    // Wima Data.
-    Q_PROPERTY(QmlObjectListModel*      visualItems
-               READ                     visualItems
-               NOTIFY                   visualItemsChanged
-               )
-    Q_PROPERTY(QmlObjectListModel*  missionItems
-               READ                 missionItems
-               NOTIFY               missionItemsChanged
-               )
-    Q_PROPERTY(QmlObjectListModel*  currentMissionItems
-               READ                 currentMissionItems
-               NOTIFY               currentMissionItemsChanged
-               )
-    Q_PROPERTY(QVariantList waypointPath
-               READ         waypointPath
-               NOTIFY       waypointPathChanged
-               )
-    Q_PROPERTY(QVariantList currentWaypointPath
-               READ         currentWaypointPath
-               NOTIFY       currentWaypointPathChanged
-               )
-    Q_PROPERTY(Fact*                    enableWimaController
-               READ enableWimaController
-               CONSTANT)
-    // Waypoint navigaton.
-    Q_PROPERTY(Fact*    overlapWaypoints
-               READ     overlapWaypoints
-               CONSTANT
-               )
-    Q_PROPERTY(Fact* maxWaypointsPerPhase
-               READ  maxWaypointsPerPhase
-               CONSTANT
-               )
-    Q_PROPERTY(Fact*    startWaypointIndex
-               READ     startWaypointIndex
-               CONSTANT
-               )
-    Q_PROPERTY(Fact*    showAllMissionItems
-               READ     showAllMissionItems
-               CONSTANT
-               )
-    Q_PROPERTY(Fact*    showCurrentMissionItems
-               READ     showCurrentMissionItems
-               CONSTANT
-               )
-    // Waypoint settings.
-    Q_PROPERTY(Fact*    flightSpeed
-               READ     flightSpeed
-               CONSTANT
-               )
-    Q_PROPERTY(Fact*    altitude
-               READ     altitude
-               CONSTANT
-               )
-    Q_PROPERTY(Fact*    arrivalReturnSpeed
-               READ     arrivalReturnSpeed
-               CONSTANT
-               )
-    // Waypoint statistics.
-    Q_PROPERTY(double   phaseDistance
-               READ     phaseDistance
-               NOTIFY   phaseDistanceChanged
-               )
-    Q_PROPERTY(double   phaseDuration
-               READ     phaseDuration
-               NOTIFY   phaseDurationChanged
-               )
-
-    // Snake
-    Q_PROPERTY(Fact*    enableSnake
-               READ     enableSnake
-               CONSTANT
-               )
-    Q_PROPERTY(int      nemoStatus
-               READ     nemoStatus
-               NOTIFY   nemoStatusChanged
-               )
-    Q_PROPERTY(QString  nemoStatusString
-               READ     nemoStatusString
-               NOTIFY   nemoStatusStringChanged
-               )
-    Q_PROPERTY(bool     snakeCalcInProgress
-               READ     snakeCalcInProgress
-               NOTIFY   snakeCalcInProgressChanged
-               )
-    Q_PROPERTY(Fact*    snakeTileWidth
-               READ     snakeTileWidth
-               CONSTANT
-               )
-    Q_PROPERTY(Fact*    snakeTileHeight
-               READ     snakeTileHeight
-               CONSTANT
-               )
-    Q_PROPERTY(Fact*    snakeMinTileArea
-               READ     snakeMinTileArea
-               CONSTANT
-               )
-    Q_PROPERTY(Fact*    snakeLineDistance
-               READ     snakeLineDistance
-               CONSTANT
-               )
-    Q_PROPERTY(Fact*    snakeMinTransectLength
-               READ     snakeMinTransectLength
-               CONSTANT
-               )
-    Q_PROPERTY(QmlObjectListModel*  snakeTiles
-               READ                 snakeTiles
-               NOTIFY               snakeTilesChanged
-               )
-    Q_PROPERTY(QVariantList snakeTileCenterPoints
-               READ         snakeTileCenterPoints
-               NOTIFY       snakeTileCenterPointsChanged
-               )
-    Q_PROPERTY(QVector<int>     nemoProgress
-               READ             nemoProgress
-               NOTIFY           nemoProgressChanged
-               )
-
-
-
-    // Property accessors
-    // Controllers.
-    PlanMasterController*       masterController       (void);
-    MissionController*          missionController      (void);
-    // Wima Data
-    QmlObjectListModel*         visualItems            (void);
-    QGCMapPolygon               joinedArea             (void) const;
-    // Waypoints.
-    QmlObjectListModel*         missionItems           (void);
-    QmlObjectListModel*         currentMissionItems    (void);
-    QVariantList                waypointPath           (void);
-    QVariantList                currentWaypointPath    (void);
-    // Settings facts.
-    Fact*                       enableWimaController   (void);
-    Fact*                       overlapWaypoints       (void);
-    Fact*                       maxWaypointsPerPhase   (void);
-    Fact*                       startWaypointIndex     (void);
-    Fact*                       showAllMissionItems    (void);
-    Fact*                       showCurrentMissionItems(void);
-    Fact*                       flightSpeed            (void);
-    Fact*                       arrivalReturnSpeed     (void);
-    Fact*                       altitude               (void);
-    // Snake settings facts.
-    Fact*                       enableSnake            (void)           { return &_enableSnake; }
-    Fact*                       snakeTileWidth         (void)           { return &_snakeTileWidth;}
-    Fact*                       snakeTileHeight        (void)           { return &_snakeTileHeight;}
-    Fact*                       snakeMinTileArea       (void)           { return &_snakeMinTileArea;}
-    Fact*                       snakeLineDistance      (void)           { return &_snakeLineDistance;}
-    Fact*                       snakeMinTransectLength (void)           { return &_snakeMinTransectLength;}
-    // Snake data.
-    QmlObjectListModel*         snakeTiles             (void)           { return _snakeTiles.QmlObjectListModel();}
-    QVariantList                snakeTileCenterPoints  (void)           { return _snakeTileCenterPoints;}
-    QVector<int>                nemoProgress           (void);
-    int                         nemoStatus             (void) const;
-    QString                     nemoStatusString       (void) const;
-    bool                        snakeCalcInProgress    (void) const;
-
-    // Smart RTL.
-    bool                        uploadOverrideRequired (void) const;
-    bool                        vehicleHasLowBattery   (void) const;
-    // Waypoint statistics.
-    double                      phaseDistance          (void) const;
-    double                      phaseDuration          (void) const;
-
-
-    // Property setters
-    void setMasterController        (PlanMasterController* masterController);
-    void setMissionController       (MissionController* missionController);
-    bool setWimaPlanData            (const WimaPlanData &planData);
-
-    // Member Methodes
-    Q_INVOKABLE WimaController *thisPointer();
-    // Waypoint navigation.
-    Q_INVOKABLE void nextPhase();
-    Q_INVOKABLE void previousPhase();
-    Q_INVOKABLE void resetPhase();
-    // Smart RTL.
-    Q_INVOKABLE void requestSmartRTL();
-    Q_INVOKABLE void initSmartRTL();
-    Q_INVOKABLE void executeSmartRTL();
-    // Other.
-    Q_INVOKABLE void removeVehicleTrajectoryHistory();
-    Q_INVOKABLE bool upload();
-    Q_INVOKABLE bool forceUpload();
-    Q_INVOKABLE void removeFromVehicle();
-
-
-    // static Members
-    static const char* areaItemsName;
-    static const char* missionItemsName;    
-    static const char* settingsGroup;
-    static const char* endWaypointIndexName;
-    static const char* enableWimaControllerName;
-    static const char* overlapWaypointsName;
-    static const char* maxWaypointsPerPhaseName;
-    static const char* startWaypointIndexName;
-    static const char* showAllMissionItemsName;
-    static const char* showCurrentMissionItemsName;
-    static const char* flightSpeedName;
-    static const char* arrivalReturnSpeedName;
-    static const char* altitudeName;
-    static const char* snakeTileWidthName;
-    static const char* snakeTileHeightName;
-    static const char* snakeMinTileAreaName;
-    static const char* snakeLineDistanceName;
-    static const char* snakeMinTransectLengthName;
-
-signals:
-    // Controllers.
-    void masterControllerChanged            (void);
-    void missionControllerChanged           (void);
-    // Wima data.
-    void visualItemsChanged                 (void);
-    // Waypoints.
-    void missionItemsChanged                (void);
-    void currentMissionItemsChanged         (void);
-    void waypointPathChanged                (void);
-    void currentWaypointPathChanged         (void);
-    // Smart RTL.
-    void smartRTLRequestConfirm             (void);
-    void smartRTLPathConfirm                (void);
-    // Upload.
-    void forceUploadConfirm                 (void);
-    // Waypoint statistics.
-    void phaseDistanceChanged               (void);
-    void phaseDurationChanged               (void);
-    // Snake.
-    void snakeConnectionStatusChanged       (void);
-    void snakeCalcInProgressChanged         (void);
-    void snakeTilesChanged                  (void);
-    void snakeTileCenterPointsChanged       (void);
-    void nemoProgressChanged                (void);
-    void nemoStatusChanged                  (void);
-    void nemoStatusStringChanged            (void);
-
-private slots:
-
-    // Waypoint navigation / helper.
-    bool _calcNextPhase         (void);
-    void _recalcCurrentPhase    (void);
-    bool _calcShortestPath      (const QGeoCoordinate &start,
-                                 const QGeoCoordinate &destination,
-                                 QVector<QGeoCoordinate> &path);
-    // Slicing parameters
-    bool _setStartIndex             (void);
-    void _updateOverlap             (void);
-    void _updateMaxWaypoints        (void);
-    // Waypoint settings.
-    void _updateflightSpeed         (void);
-    void _updateArrivalReturnSpeed  (void);
-    void _updateAltitude            (void);
-    // Waypoint Statistics.
-    void _setPhaseDistance  (double distance);
-    void _setPhaseDuration  (double duration);
-    // SMART RTL
-    void    _checkBatteryLevel                  (void);
-    bool    _checkSmartRTLPreCondition          (QString &errorString);
-    void    _initSmartRTL                       ();
-    void    _smartRTLCleanUp                    (bool flying);
-    // Snake.
-    void _setSnakeCalcInProgress             (bool inProgress);
-    void _updateSnakeData                    ();
-    void _initStartSnakeWorker               ();
-    void _switchSnakeManager                 (QVariant variant);
-    // Periodic tasks.
-    void _eventTimerHandler                  (void);
-    // Waypoint manager handling.
-    void _switchWaypointManager(WaypointManager::ManagerBase &manager);
-
-private:
-    using StatusMap = std::map<int, QString>;
-
-    // Controllers.
-    PlanMasterController   *_masterController;
-    MissionController      *_missionController;
-
-    // Wima Data.
-    QmlObjectListModel      _areas; // contains all visible areas
-    WimaJoinedAreaData      _joinedArea; // joined area fromed by opArea, serArea, _corridor
-    WimaMeasurementAreaData _measurementArea; // measurement area
-    WimaServiceAreaData     _serviceArea; // area for supplying
-    WimaCorridorData        _corridor; // corridor connecting opArea and serArea
-    bool                    _localPlanDataValid;
-
-    // Waypoint Managers.
-    WaypointManager::AreaInterface      _areaInterface;
-    WaypointManager::Settings           _managerSettings;
-    WaypointManager::DefaultManager     _defaultManager;
-    WaypointManager::DefaultManager     _snakeManager;
-    WaypointManager::RTLManager         _rtlManager;
-    WaypointManager::ManagerBase       *_currentManager;
-    using ManagerList = QList<WaypointManager::ManagerBase*>;
-    ManagerList                         _managerList;
-
-    // Settings Facts.
-    QMap<QString, FactMetaData*> _metaDataMap;
-    SettingsFact                 _enableWimaController; // enables or disables the wimaControler
-    SettingsFact                 _overlapWaypoints; // determines the number of overlapping waypoints between two consecutive mission phases
-    SettingsFact                 _maxWaypointsPerPhase; // determines the maximum number waypoints per phase
-    SettingsFact                 _nextPhaseStartWaypointIndex; // index (displayed on the map, -1 to get index of item in _missionItems) of the mission item
-                                                      // defining the first element of the next phase
-    SettingsFact                 _showAllMissionItems; // bool value, Determines whether the mission items of the overall mission are displayed or not.
-    SettingsFact                 _showCurrentMissionItems; // bool value, Determines whether the mission items of the current mission phase are displayed or not.
-    SettingsFact                 _flightSpeed; // mission flight speed
-    SettingsFact                 _arrivalReturnSpeed; // arrival and return path speed
-    SettingsFact                 _altitude; // mission altitude
-    SettingsFact                 _enableSnake; // Enable Snake (see snake.h)
-    SettingsFact                 _snakeTileWidth;
-    SettingsFact                 _snakeTileHeight;
-    SettingsFact                 _snakeMinTileArea;
-    SettingsFact                 _snakeLineDistance;
-    SettingsFact                 _snakeMinTransectLength;
-
-    // Smart RTL.
-    QTimer          _smartRTLTimer;
-    bool            _lowBatteryHandlingTriggered;
-
-    // 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;
-    SnakeDataManager             _snakeWorker;
-    GeoPoint                _snakeOrigin;
-    GeoPolygonArray         _snakeTiles; // tiles
-    PolygonArray            _snakeTilesLocal; // tiles local coordinate system
-    QVariantList            _snakeTileCenterPoints;
-    QNemoProgress           _nemoProgress; // measurement progress
-    QNemoHeartbeat          _nemoHeartbeat; // measurement progress
-    int                     _fallbackStatus;
-    ROSBridgePtr            _pRosBridge;
-    bool                    _bridgeSetupDone;
-    static StatusMap        _nemoStatusMap;
-
-    // Periodic tasks.
-    QTimer                  _eventTimer;
-
-};
-
-

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

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

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

-#ifndef MESSAGES_H
-#define MESSAGES_H
-
-#include <iostream>
-#include <vector>
-#include <boost/type_traits/remove_cv.hpp>
-#include <boost/type_traits/remove_reference.hpp>
-
-#include "ros_bridge/rapidjson/include/rapidjson/rapidjson.h"
-#include "ros_bridge/rapidjson/include/rapidjson/document.h"
-
-#include "utilities.h"
-#include "MessageTraits.h"
-#include <ostream>
-
-namespace ROSBridge {
-
-//! @brief Namespace containing methodes for Json generation.
-namespace JsonMethodes {
-
-//! @brief Namespace containing methodes for std_msgs generation.
-namespace StdMsgs {
-//! @brief Namespace containing methodes for std_msgs/Time message generation.
-namespace Time {    
-    template<class TimeType>
-    bool toJson(const TimeType &time,
-                rapidjson::Value &value,
-                rapidjson::Document::AllocatorType &allocator)
-    {
-        value.AddMember("secs", rapidjson::Value().SetUint(uint32_t(time.secs())), allocator);
-        value.AddMember("nsecs", rapidjson::Value().SetUint(uint32_t(time.nSecs())), allocator);
-
-        return true;
-    }
-
-    template<class TimeType>
-    bool fromJson(const rapidjson::Value &value,
-                  TimeType &time)
-    {
-        if (!value.HasMember("secs") || !value["secs"].IsUint()){
-            assert(false);
-            return false;
-        }
-        if (!value.HasMember("nsecs")|| !value["nsecs"].IsUint()){
-            assert(false);
-            return false;
-        }
-        time.setSecs(value["secs"].GetUint());
-        time.setNSecs(value["nsecs"].GetUint());
-
-        return true;
-    }
-
-} // Time
-
-//! @brief Namespace containing methodes for std_msgs/Header message generation.
-namespace Header {
-
-    template <class HeaderType>
-    bool toJson(const HeaderType &header,
-                rapidjson::Value &value,
-                rapidjson::Document::AllocatorType &allocator)     {
-        value.AddMember("seq", rapidjson::Value().SetUint(uint32_t(header.seq())), allocator);
-
-        rapidjson::Value stamp(rapidjson::kObjectType);
-        if (!Time::toJson(header.stamp(), stamp, allocator)){
-            assert(false);
-            return false;
-        }
-        value.AddMember("stamp", stamp, allocator);
-
-        value.AddMember("frame_id",
-                      rapidjson::Value().SetString(header.frameId().data(),
-                                                   header.frameId().length(),
-                                                   allocator),
-                      allocator);
-
-        return true;
-    }
-
-    template <class HeaderType>
-    bool fromJson(const rapidjson::Value &value,
-                  HeaderType &header)     {
-        if (!value.HasMember("seq")|| !value["seq"].IsUint()){
-            assert(false);
-            return false;
-        }
-        if (!value.HasMember("stamp")){
-            assert(false);
-            return false;
-        }
-        if (!value.HasMember("frame_id")|| !value["frame_id"].IsString()){
-            assert(false);
-            return false;
-        }
-        header.setSeq(value["seq"].GetUint());
-        decltype(header.stamp()) time;
-        if (!Time::fromJson(value["stamp"], time)){
-            assert(false);
-            return false;
-        }
-        header.setStamp(time);
-        header.setFrameId(value["frame_id"].GetString());
-
-        return true;
-    }
-
-} // Header
-} // StdMsgs
-
-
-//! @brief Namespace containing methodes for geometry_msgs generation.
-namespace GeometryMsgs {
-//! @brief Namespace containing methodes for geometry_msgs/Point32 message generation.
-namespace Point32 {
-using namespace ROSBridge::traits;
-
-        namespace det { //detail
-
-            template <class T>
-            auto getZ(const T &p, Type2Type<Has3Components>)
-            {
-                return p.z();
-            }
-
-            template <class T>
-            auto getZ(const T &p, Type2Type<Has2Components>)
-            {
-                (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 <class T>
-            bool setZ(const rapidjson::Value &doc, const T &p, Type2Type<Has2Components>)
-            {
-                (void)doc;
-                (void)p;
-                return true;
-            }
-        }
-
-        template <class T>
-        bool toJson(const T&p, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
-        {
-            value.AddMember("x", rapidjson::Value().SetFloat(p.x()), allocator);
-            value.AddMember("y", rapidjson::Value().SetFloat(p.y()), allocator);
-
-            typedef typename Select<HasMemberZ<T>::value, Has3Components, Has2Components>::Result
-                    Components; // Check if PointType has 2 or 3 dimensions.
-            auto z = det::getZ(p, Type2Type<Components>()); // If T has no member z() replace it by 0.
-
-            value.AddMember("z", rapidjson::Value().SetFloat(z), allocator);
-
-            return true;
-        }
-
-        template <class PointType>
-        bool fromJson(const rapidjson::Value &value,
-                      PointType &p)     {
-            if (!value.HasMember("x") || !value["x"].IsFloat()){
-                assert(false);
-                return false;
-            }
-            if (!value.HasMember("y") || !value["y"].IsFloat()){
-                assert(false);
-                return false;
-            }
-            if (!value.HasMember("z") || !value["z"].IsFloat()){
-                assert(false);
-                return false;
-            }
-
-            p.setX(value["x"].GetFloat());
-            p.setY(value["y"].GetFloat());
-            typedef typename Select<HasMemberSetZ<PointType>::value, Has3Components, Has2Components>::Result
-                    Components; // Check if PointType has 2 or 3 dimensions.
-            (void)det::setZ(value["z"], p,  Type2Type<Components>()); // If PointType has no member z() discard doc["z"].
-
-            return true;
-        }
-
-} // Point32
-
-//! @brief Namespace containing methodes for geometry_msgs/Polygon message generation.
-namespace Polygon {
-
-template <class PolygonType>
-bool toJson(const PolygonType &poly, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
-{
-    rapidjson::Value points(rapidjson::kArrayType);
-
-    for(unsigned long i=0; i < std::uint64_t(poly.points().size()); ++i) {
-        rapidjson::Document point(rapidjson::kObjectType);
-        if ( !Point32::toJson(poly.points()[i], point, allocator) ){
-            assert(false);
-            return false;
-        }
-        points.PushBack(point, allocator);
-    }
-
-    value.AddMember("points", points, allocator);
-
-    return true;
-}
-
-template <class PolygonType>
-bool fromJson(const rapidjson::Value &value, PolygonType &poly)
-{
-    if (!value.HasMember("points") || !value["points"].IsArray()){
-        assert(false);
-        return false;
-    }
-    const auto &jsonArray = value["points"].GetArray();
-    poly.points().clear();
-    poly.points().reserve(jsonArray.Size());
-    typedef decltype (poly.points()[0]) PointTypeCVR;
-    typedef typename boost::remove_cv<typename boost::remove_reference<PointTypeCVR>::type>::type PointType;
-    for (long i=0; i < jsonArray.Size(); ++i){
-        PointType pt;
-        if ( !Point32::fromJson(jsonArray[i], pt) ){
-            assert(false);
-            return false;
-        }
-        poly.points().push_back(std::move(pt));
-    }
-    return true;
-}
-} // namespace Polygon
-
-
-//! @brief Namespace containing methodes for geometry_msgs/PolygonStamped message generation.
-namespace PolygonStamped {
-using namespace ROSBridge::JsonMethodes::StdMsgs;
-
-
-template <class PolyStamped>
-bool toJson(const PolyStamped &polyStamped, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
-{
-    return toJson(polyStamped.polygon(), polyStamped.header(), value, allocator);
-}
-
-template <class PolygonType, class HeaderType>
-bool toJson(const PolygonType &poly, const HeaderType &h, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
-{
-    rapidjson::Document header(rapidjson::kObjectType);
-    if (!Header::toJson(h, header, allocator)){
-        assert(false);
-        return false;
-    }
-    rapidjson::Document polygon(rapidjson::kObjectType);
-    if (!Polygon::toJson(poly, polygon, allocator)){
-        assert(false);
-        return false;
-    }
-    value.AddMember("header", header, allocator);
-    value.AddMember("polygon", polygon, allocator);
-
-    return true;
-}
-
-namespace det {
-template <class PolygonStampedType>
-bool setHeader(const rapidjson::Value &doc, PolygonStampedType &polyStamped, Int2Type<1>) { // polyStamped.setHeader() exists
-    typedef decltype (polyStamped.header()) HeaderTypeCVR;
-    typedef typename boost::remove_cv<typename boost::remove_reference<HeaderTypeCVR>::type>::type HeaderType;
-    HeaderType header;
-    bool ret = Header::fromJson(doc, header);
-    polyStamped.header() = header;
-    return ret;
-}
-template <class PolygonStampedType>
-bool setHeader(const rapidjson::Value &doc, PolygonStampedType &polyStamped, Int2Type<0>) { // polyStamped.setHeader() does not exists
-    (void)doc;
-    (void)polyStamped;
-    return true;
-}
-} // namespace det
-
-template <class PolygonType, class HeaderType>
-bool fromJson(const rapidjson::Value &value, PolygonType &polyStamped)
-{
-    if ( !value.HasMember("header") ){
-        assert(false);
-        return false;
-    }
-    if ( !value.HasMember("polygon") ){
-        assert(false);
-        return false;
-    }
-
-
-    typedef traits::HasMemberSetHeader<PolygonType> HasHeader;
-    if ( !det::setHeader(value["header"], polyStamped, Int2Type<HasHeader::value>())){
-        assert(false);
-        return false;
-    }
-
-    if ( !Polygon::fromJson(value["polygon"], polyStamped.polygon()) ){
-        assert(false);
-        return false;
-    }
-
-    return true;
-}
-} // namespace PolygonStamped
-
-} // namespace GeometryMsgs
-
-
-//! @brief Namespace containing methodes for geographic_msgs generation.
-namespace GeographicMsgs {
-//! @brief Namespace containing methodes for geographic_msgs/GeoPoint message generation.
-namespace GeoPoint {
-using namespace ROSBridge::traits;
-
-        namespace det { //detail
-
-            template <class T>
-            auto getAltitude(const T &p, Type2Type<Has3Components>)
-            {
-                return p.altitude();
-            }
-
-            template <class T>
-            auto getAltitude(const T &p, Type2Type<Has2Components>)
-            {
-                (void)p;
-                return 0.0;
-            }
-
-            template <class T>
-            void setAltitude(const rapidjson::Value &doc, T &p, Type2Type<Has3Components>)
-            {
-                p.setAltitude(doc.GetFloat());
-            }
-
-            template <class T>
-            void setAltitude(const rapidjson::Value &doc, T &p, Type2Type<Has2Components>)
-            {
-                (void)doc;
-                (void)p;
-            }
-        } // namespace det
-
-        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 Select<HasMemberAltitude<T>::value, Has3Components, Has2Components>::Result
-                    Components; // Check if PointType has 2 or 3 dimensions.
-            auto altitude = det::getAltitude(p, Type2Type<Components>()); // If T has no member altitude() replace it by 0.0;
-
-            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 Select<HasMemberSetAltitude<PointType>::value, Has3Components, Has2Components>::Result
-                    Components; // Check if PointType has 2 or 3 dimensions.
-            det::setAltitude(value["altitude"], p,  Type2Type<Components>()); // If T has no member altitude() discard doc["altitude"];
-
-            return true;
-        }
-
-} // GeoPoint
-} // GeographicMsgs
-
-
-//! @brief Namespace containing methodes for jsk_recognition_msgs generation.
-namespace JSKRecognitionMsgs {
-//! @brief Namespace containing methodes for jsk_recognition_msgs/PolygonArray message generation.
-namespace PolygonArray {
-using namespace ROSBridge::traits;
-using namespace ROSBridge::JsonMethodes::StdMsgs;
-using namespace ROSBridge::JsonMethodes::GeometryMsgs;
-
-    namespace PAdetail {
-        //! Helper functions to generate Json entries for labels and likelihood.
-
-        //! \note \p p has member \fn labels().
-        template <class PolygonArrayType, int  k>
-        void labelsToJson(const PolygonArrayType &p, rapidjson::Value &labels, rapidjson::Document::AllocatorType &allocator, Int2Type<k>){
-            for(unsigned long i=0; i < (unsigned long)p.labels().size(); ++i)
-                labels.PushBack(rapidjson::Value().SetUint(p.labels()[i]), allocator);
-        }
-
-        //! \note \p p has no member \fn labels().
-        template <class PolygonArrayType>
-        void labelsToJson(const PolygonArrayType &p, rapidjson::Value &labels, rapidjson::Document::AllocatorType &allocator, Int2Type<0>){
-            for(unsigned long i=0; i < (unsigned long)(p.polygons().size()); ++i)
-                labels.PushBack(rapidjson::Value().SetUint(0), allocator); // use zero!
-        }        
-
-        //! \note \p p has member \fn likelihood().
-        template <class PolygonArrayType, int  k>
-        void likelihoodToJson(const PolygonArrayType &p,
-                              rapidjson::Value &likelyhood,
-                              rapidjson::Document::AllocatorType &allocator,
-                              Int2Type<k>){
-            p.likelyhood().clear();
-            for(unsigned long i=0; i < (unsigned long)p.likelyhood().size(); ++i)
-                likelyhood.PushBack(rapidjson::Value().SetFloat(p.likelyhood()[i]), allocator);
-        }
-
-        //! \note \p p has no member \fn likelihood().
-        template <class PolygonArrayType>
-        void likelihoodToJson(const PolygonArrayType &p,
-                              rapidjson::Value &likelyhood,
-                              rapidjson::Document::AllocatorType &allocator,
-                              Int2Type<0>){
-            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>){
-            p.labels().clear();
-            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;
-        }
-    }
-
-
-    //!
-    //! Create PolygonArray message from \p p. \p p contains a header.
-    //! \param p Class implementing the PolygonArrayType interface.
-    //! \param doc Rapidjson document used to store the PolygonArray message.
-    //! \param allocator Allocator used by doc. Can be obtained e.g. by calling doc.getAllocator().
-    //!
-    //! \note The \fn labels() and \fn likelihood() members are optinal. If any of them is missing they
-    //! will be replaced by arrays filled with zero and size p.polygons.size().
-    //!
-    //! \note If this function is called p.polygons[i] (entries implement the the PolygonStampedGroup interface)
-    //! must contain a header.
-    template <class PolygonArrayType>
-    bool toJson(const PolygonArrayType &pArray, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
-    {
-        rapidjson::Document header(rapidjson::kObjectType);
-        if (Header::toJson(pArray.header(), header, allocator)){
-            assert(false);
-            return false;
-        }
-        value.AddMember("header", header, allocator);
-
-        rapidjson::Value polygons(rapidjson::kArrayType);
-        for(unsigned long i=0; i < pArray.polygons().size(); ++i){
-            rapidjson::Document polygon(rapidjson::kObjectType);
-            if (!PolygonStamped::toJson(pArray.polygons()[i], polygon, allocator)){
-                assert(false);
-                return false;
-            }
-            polygons.PushBack(polygon, allocator);
-        }
-        value.AddMember("polygons", polygons, allocator);
-
-
-        rapidjson::Value labels(rapidjson::kArrayType);
-        typedef HasMemberLabels<PolygonArrayType> HasLabels;
-        PAdetail::labelsToJson(pArray, labels, allocator, Int2Type<HasLabels::value>());
-        value.AddMember("labels", labels, allocator);
-
-
-        rapidjson::Value likelihood(rapidjson::kArrayType);
-        typedef HasMemberLikelihood<PolygonArrayType> HasLikelihood;
-        PAdetail::likelihoodToJson(pArray, likelihood, allocator, Int2Type<HasLikelihood::value>());
-        value.AddMember("likelihood", likelihood, allocator);
-
-        return true;
-    }
-
-
-    //!
-    //! Create PolygonArray message from \p p and \p h. \p p doesn't have it's own header.
-    //! \param p Class implementing the PolygonArrayType interface.
-    //! \param h Class implementing the HeaderType interface.
-    //! \param doc Rapidjson document used to store the PolygonArray message.
-    //! \param allocator Allocator used by doc. Can be obtained e.g. by calling doc.getAllocator().
-    //!
-    //! \note The \fn labels() and \fn likelihood() members are optinal. If any of them is missing they
-    //! will be replaced by arrays filled with zero and size p.polygons.size().
-    //!
-    //! \note If this function is called the headers in p.polygons[i] (entries implement the the PolygonStampedGroup interface)
-    //! are ignored.
-    template <class PolygonArrayType, class HeaderType>
-    bool toJson(const PolygonArrayType &p, const HeaderType &h, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
-    {
-        rapidjson::Document header(rapidjson::kObjectType);
-        if (!Header::toJson(h, header, allocator)){
-            assert(false);
-            return false;
-        }
-        value.AddMember("header", header, allocator);
-
-        rapidjson::Value polygons(rapidjson::kArrayType);
-        for(unsigned long i=0; i < (unsigned long)(p.polygons().size()); ++i){
-            rapidjson::Document polygon(rapidjson::kObjectType);
-            if (!PolygonStamped::toJson(p.polygons()[i].polygon(), h, polygon, allocator)){
-                assert(false);
-                return false;
-            }
-            polygons.PushBack(polygon, allocator);
-        }
-        value.AddMember("polygons", polygons, allocator);
-
-
-        rapidjson::Value labels(rapidjson::kArrayType);
-        typedef HasMemberLabels<PolygonArrayType> HasLabels;
-        PAdetail::labelsToJson(p, labels, allocator, Int2Type<HasLabels::value>());
-        value.AddMember("labels", labels, allocator);
-
-
-        rapidjson::Value likelihood(rapidjson::kArrayType);
-        typedef HasMemberLikelihood<PolygonArrayType> HasLikelihood;
-        PAdetail::likelihoodToJson(p, likelihood, allocator, Int2Type<HasLikelihood::value>());
-        value.AddMember("likelihood", likelihood, allocator);
-
-        return true;
-    }
-
-    template <class PolygonArrayType>
-    bool fromJson(const rapidjson::Value &value, PolygonArrayType &p)
-    {
-        if ( !value.HasMember("header")){
-            assert(false);
-            return false;
-        }
-        if ( !value.HasMember("polygons") || !value["polygons"].IsArray() ){
-            assert(false);
-            return false;
-        }
-        if ( !value.HasMember("labels") || !value["labels"].IsArray() ){
-            assert(false);
-            return false;
-        }
-        if ( !value.HasMember("likelihood") || !value["likelihood"].IsArray() ){
-            assert(false);
-            return false;
-        }
-
-
-        typedef traits::HasMemberHeader<PolygonArrayType> HasHeader;
-        if ( !PolygonStamped::det::setHeader(value["header"], p, Int2Type<HasHeader::value>())){
-            assert(false);
-            return false;
-        }
-
-
-        const auto &polyStampedJson = value["polygons"];
-        p.polygons().clear();
-        p.polygons().reserve(polyStampedJson.Size());
-        typedef decltype (p.polygons()[0]) PolyStampedCVR;
-        typedef typename boost::remove_cv<typename boost::remove_reference<PolyStampedCVR>::type>::type
-                PolyStamped;
-        for (unsigned int i=0; i < polyStampedJson.Size(); ++i) {
-            if ( !polyStampedJson[i].HasMember("header") ){
-                assert(false);
-                return false;
-            }
-            PolyStamped polyStamped;
-
-            if ( !PolygonStamped::det::setHeader(polyStampedJson[i]["header"], polyStamped, Int2Type<HasHeader::value>())){
-                assert(false);
-                return false;
-            }
-
-            if ( !Polygon::fromJson(polyStampedJson[i]["polygon"], polyStamped.polygon())){
-                assert(false);
-                return false;
-            }
-
-            p.polygons().push_back(std::move(polyStamped));
-        }
-
-        typedef traits::HasMemberLabels<PolygonArrayType> HasLabels;
-        PAdetail::setLabels(value["labels"], p, Int2Type<HasLabels::value>());
-
-        typedef traits::HasMemberLikelihood<PolygonArrayType> HasLikelihood;
-        PAdetail::setLikelihood(value["likelihood"], p, Int2Type<HasLikelihood::value>());
-
-        return true;
-    }
-
-} // end namespace PolygonArray
-} // namespace JSKRekognitionMsgs
-
-
-//! @brief Namespace containing methodes for nemo_msgs generation.
-namespace NemoMsgs {
-//! @brief Namespace containing methodes for nemo_msgs/Progress generation.
-namespace Progress {
-    template <class ProgressType>
-    bool toJson(const ProgressType &p, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
-    {
-        rapidjson::Value progressJson(rapidjson::kArrayType);
-        for(unsigned long i=0; i < std::uint64_t(p.progress().size()); ++i){
-            progressJson.PushBack(rapidjson::Value().SetInt(std::int32_t(p.progress()[i])), allocator);
-        }
-        value.AddMember("progress", progressJson, allocator);
-        return true;
-    }
-
-    template <class ProgressType>
-    bool fromJson(const rapidjson::Value &value, ProgressType &p)
-    {
-        if (!value.HasMember("progress") || !value["progress"].IsArray()){
-            assert(false);
-            return false;
-        }
-
-        const auto& jsonProgress = value["progress"];
-        unsigned long sz = jsonProgress.Size();
-        p.progress().clear();
-        p.progress().reserve(sz);
-        for (unsigned long i=0; i < sz; ++i)
-            p.progress().push_back(std::int32_t(jsonProgress[i].GetInt()));
-        return true;
-    }
-} // namespace Progress
-
-//! @brief Namespace containing methodes for nemo_msgs/Heartbeat generation.
-namespace Heartbeat {
-    template <class HeartbeatType>
-    bool toJson(const HeartbeatType &heartbeat, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
-    {
-        value.AddMember("status", std::int32_t(heartbeat.status()), allocator);
-        return true;
-    }
-
-    template <class HeartbeatType>
-    bool fromJson(const rapidjson::Value &value, HeartbeatType &heartbeat)
-    {
-        if (!value.HasMember("status") || !value["status"].IsInt()){
-            assert(false);
-            return false;
-        }
-
-        heartbeat.setStatus(value["status"].GetInt());
-        return true;
-    }
-} // namespace Heartbeat
-} // namespace NemoMsgs
-} // namespace JsonMethodes
-} // namespace ROSBridge
-
-#endif // MESSAGES_H

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/RosBridgeClient.cpp

@@ -18,18 +18,18 @@ struct Task{
 
 void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shared_ptr<WsClient> client, const std::__cxx11::string &message)
 {
-#ifndef DEBUG
+#ifndef ROS_BRIDGE_DEBUG
     (void)client_name;
 #endif
     if (!client->on_open)
     {
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         client->on_open = [client_name, message](std::shared_ptr<WsClient::Connection> connection) {
 #else
         client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
 #endif
 
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
             std::cout << client_name << ": Opened connection" << std::endl;
             std::cout << client_name << ": Sending message: " << message << std::endl;
 #endif
@@ -37,7 +37,7 @@ void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shar
         };
     }
 
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     if (!client->on_message)
     {
         client->on_message = [client_name](std::shared_ptr<WsClient::Connection> /*connection*/, std::shared_ptr<WsClient::InMessage> in_message) {
@@ -61,21 +61,21 @@ void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shar
     }
 
 #endif
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     std::thread client_thread([client_name, client]() {
 #else
     std::thread client_thread([client]() {
 #endif
         client->start();
 
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         std::cout << client_name << ": Terminated" << std::endl;
 #endif
         client->on_open = NULL;
         client->on_message = NULL;
         client->on_close = NULL;
         client->on_error = NULL;
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         std::cout << client_name << " thread end" << std::endl;
 #endif
     });
@@ -122,10 +122,6 @@ void RosbridgeWsClient::run()
             // ====================================================================================
             if ( std::chrono::high_resolution_clock::now() > poll_time_point) {
                 poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval;
-#ifdef DEBUG
-                std::cout << "Starting new poll." << std::endl;
-                std::cout << "connected: " << this->isConnected->load() << std::endl;
-#endif
                 std::string reset_status_task_name = "reset_status_task";
                 // Add status task if necessary.
                 auto const it = std::find_if(task_list.begin(), task_list.end(),
@@ -133,16 +129,10 @@ void RosbridgeWsClient::run()
                     return t.name == reset_status_task_name;
                 });
                 if ( it == task_list.end() ){
-#ifdef DEBUG
-                    std::cout << "Adding status_task" << std::endl;
-#endif
                     // Check connection status.
                     auto status_set = std::make_shared<std::atomic_bool>(false);
                     auto status_client = std::make_shared<WsClient>(this->server_port_path);
                     status_client->on_open = [status_set, this](std::shared_ptr<WsClient::Connection>) {
-#ifdef DEBUG
-                            std::cout << "status_client opened" << std::endl;
-#endif
                             this->isConnected->store(true);
                             status_set->store(true);
                     };
@@ -190,16 +180,10 @@ void RosbridgeWsClient::run()
                     });
                     if ( topics_it == task_list.end() ){
                         // Call /rosapi/topics service.
-#ifdef DEBUG
-                        std::cout << "Adding reset_topics_task" << std::endl;
-#endif
                         auto topics_set = std::make_shared<std::atomic_bool>(false);
                         this->callService("/rosapi/topics", [topics_set, this](
                                           std::shared_ptr<WsClient::Connection> connection,
                                           std::shared_ptr<WsClient::InMessage> in_message){
-#ifdef DEBUG
-                            std::cout << "/rosapi/topics: " << in_message->string() << std::endl;
-#endif
                             std::unique_lock<std::mutex> lk(this->mutex);
                             this->available_topics = in_message->string();
                             lk.unlock();
@@ -241,16 +225,10 @@ void RosbridgeWsClient::run()
                     });
                     if ( services_it == task_list.end() ){
                         // Call /rosapi/services service.
-#ifdef DEBUG
-                        std::cout << "Adding reset_services_task" << std::endl;
-#endif
                         auto services_set = std::make_shared<std::atomic_bool>(false);
                         this->callService("/rosapi/services", [this, services_set](
                                           std::shared_ptr<WsClient::Connection> connection,
                                           std::shared_ptr<WsClient::InMessage> in_message){
-#ifdef DEBUG
-                            std::cout << "/rosapi/services: " << in_message->string() << std::endl;
-#endif
                             std::unique_lock<std::mutex> lk(this->mutex);
                             this->available_services = in_message->string();
                             lk.unlock();
@@ -295,26 +273,14 @@ void RosbridgeWsClient::run()
             // Process tasks.
             // ====================================================================================
             for ( auto task_it = task_list.begin(); task_it != task_list.end(); ){
-#ifdef DEBUG
-                std::cout << "processing task: " << task_it->name << std::endl;
-#endif
                 if ( !task_it->expired() ){
                     if ( task_it->ready() ){
-#ifdef DEBUG
-                        std::cout << "executing task: " << task_it->name << std::endl;
-#endif
                         task_it->execute();
                         task_it = task_list.erase(task_it);
                     } else {
-#ifdef DEBUG
-                        std::cout << "noting to do for task: " << task_it->name << std::endl;
-#endif
                         ++task_it;
                     }
                 } else {
-#ifdef DEBUG
-                    std::cout << "task expired: " << task_it->name << std::endl;
-#endif
                     task_it->clear_up();
                     task_it = task_list.erase(task_it);
                 }
@@ -328,7 +294,7 @@ void RosbridgeWsClient::run()
             task_it->clear_up();
         }
         task_list.clear();
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         std::cout << "periodic thread end" << std::endl;
 #endif
     });
@@ -349,9 +315,14 @@ void RosbridgeWsClient::reset()
     unsubscribeAll();
     unadvertiseAll();
     unadvertiseAllServices();
-    for (auto& client : client_map)
-    {
-        removeClient(client.first);
+
+    std::unique_lock<std::mutex> lk(mutex);
+    for (auto it = client_map.begin(); it != client_map.end(); ) {
+        std::string client_name = it->first;
+        lk.unlock();
+        removeClient(client_name);
+        lk.lock();
+        it = client_map.begin();
     }
 }
 
@@ -363,7 +334,7 @@ void RosbridgeWsClient::addClient(const std::string &client_name)
     {
         client_map[client_name] = std::make_shared<WsClient>(server_port_path);
     }
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     else
     {
         std::cerr << client_name << " has already been created" << std::endl;
@@ -391,7 +362,7 @@ void RosbridgeWsClient::stopClient(const std::string &client_name)
         // Stop the client asynchronously in 100 ms.
         // This is to ensure, that all threads involving the client have been launched.
         std::shared_ptr<WsClient> client = it->second;
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         std::thread t([client, client_name](){
 #else
         std::thread t([client](){
@@ -403,14 +374,13 @@ void RosbridgeWsClient::stopClient(const std::string &client_name)
 //            client->on_message = NULL;
 //            client->on_close = NULL;
 //            client->on_error = NULL;
-#ifdef DEBUG
-            std::cout << "removeClient thread: " << client_name << " reference count: " << client.use_count() << std::endl;
+#ifdef ROS_BRIDGE_DEBUG
             std::cout << client_name << " has been removed" << std::endl;
 #endif
         });
         t.detach();
     }
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     else
     {
         std::cerr << client_name << " has not been created" << std::endl;
@@ -428,7 +398,7 @@ void RosbridgeWsClient::removeClient(const std::string &client_name)
         {
             client_map.erase(it);
         }
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         else
         {
             std::cerr << client_name << " has not been created" << std::endl;
@@ -448,15 +418,24 @@ std::string RosbridgeWsClient::getAdvertisedServices(){
 }
 
 bool RosbridgeWsClient::topicAvailable(const std::string &topic){
-#ifdef DEBUG
+    std::lock_guard<std::mutex> lk(mutex);
+#ifdef ROS_BRIDGE_DEBUG
     std::cout << "checking if topic " << topic << " is available" << std::endl;
+    std::cout << "available topics: " << available_topics << std::endl;
 #endif
     size_t pos;
     {
-        std::lock_guard<std::mutex> lk(mutex);
         pos = available_topics.find(topic);
     }
-    return pos != std::string::npos ? true : false;
+    bool ret = pos != std::string::npos ? true : false;
+#ifdef ROS_BRIDGE_DEBUG
+    if ( ret ){
+        std::cout << "topic " << topic << " is available" << std::endl;
+    } else {
+        std::cout << "topic " << topic << " is not available" << std::endl;
+    }
+#endif
+    return ret;
 }
 
 void RosbridgeWsClient::advertise(const std::string &client_name, const std::string &topic, const std::string &type, const std::string &id)
@@ -471,7 +450,7 @@ void RosbridgeWsClient::advertise(const std::string &client_name, const std::str
             return topic == std::get<integral(EntryEnum::ServiceTopicName)>(td);
         });
         if ( it_ser_top != service_topic_list.end()){
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
             std::cerr << "topic: " << topic << " already advertised" << std::endl;
 #endif
             return;
@@ -487,13 +466,13 @@ void RosbridgeWsClient::advertise(const std::string &client_name, const std::str
         }
         message = "{" + message + "}";
 
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         client->on_open = [this, topic, message, client_name](std::shared_ptr<WsClient::Connection> connection) {
 #else
         client->on_open = [this, topic, message](std::shared_ptr<WsClient::Connection> connection) {
 #endif
 
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
             std::cout << client_name << ": Opened connection" << std::endl;
             std::cout << client_name << ": Sending message: " << message << std::endl;
 #endif
@@ -502,7 +481,7 @@ void RosbridgeWsClient::advertise(const std::string &client_name, const std::str
 
         start(client_name, client, message);
     }
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     else
     {
         std::cerr << client_name << "has not been created" << std::endl;
@@ -518,7 +497,7 @@ void RosbridgeWsClient::unadvertise(const std::string &topic, const std::string
         return topic == std::get<integral(EntryEnum::ServiceTopicName)>(td);
     });
     if ( it_ser_top == service_topic_list.end()){
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         std::cerr << "topic: " << topic << " not advertised" << std::endl;
 #endif
         return;
@@ -534,13 +513,13 @@ void RosbridgeWsClient::unadvertise(const std::string &topic, const std::string
 
     std::string client_name = "topic_unadvertiser" + topic;
     auto client = std::make_shared<WsClient>(server_port_path);
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     client->on_open = [client_name, message](std::shared_ptr<WsClient::Connection> connection) {
 #else
     client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
 #endif
 
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         std::cout << client_name << ": Opened connection" << std::endl;
         std::cout << client_name << ": Sending message: " << message << std::endl;
 #endif
@@ -569,7 +548,7 @@ void RosbridgeWsClient::publish(const std::string &topic, const rapidjson::Docum
         return topic == std::get<integral(EntryEnum::ServiceTopicName)>(td);
     });
     if ( it_ser_top == service_topic_list.end() ){
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         std::cerr << "topic: " << topic << " not yet advertised" << std::endl;
 #endif
         return;
@@ -588,12 +567,12 @@ void RosbridgeWsClient::publish(const std::string &topic, const rapidjson::Docum
     message = "{" + message + "}";
 
     std::shared_ptr<WsClient> publish_client = std::make_shared<WsClient>(server_port_path);
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     publish_client->on_open = [message, client_name](std::shared_ptr<WsClient::Connection> connection) {
 #else
     publish_client->on_open = [message, client_name](std::shared_ptr<WsClient::Connection> connection) {
 #endif
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         std::cout << client_name << ": Opened connection" << std::endl;
         std::cout << client_name << ": Sending message." << std::endl;
         std::cout << client_name << ": Sending message: " << message << std::endl;
@@ -619,7 +598,7 @@ void RosbridgeWsClient::subscribe(const std::string &client_name, const std::str
             return topic == std::get<integral(EntryEnum::ServiceTopicName)>(td);
         });
         if ( it_ser_top != service_topic_list.end()){
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
             std::cerr << "topic: " << topic << " already advertised" << std::endl;
 #endif
             return;
@@ -659,7 +638,7 @@ void RosbridgeWsClient::subscribe(const std::string &client_name, const std::str
         client->on_message = callback;
         this->start(client_name, client, message); // subscribe to topic
     }
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     else
     {
         std::cerr << client_name << "has not been created" << std::endl;
@@ -675,7 +654,7 @@ void RosbridgeWsClient::unsubscribe(const std::string &topic, const std::string
         return topic == std::get<integral(EntryEnum::ServiceTopicName)>(td);
     });
     if ( it_ser_top == service_topic_list.end()){
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         std::cerr << "topic: " << topic << " not advertised" << std::endl;
 #endif
         return;
@@ -691,13 +670,13 @@ void RosbridgeWsClient::unsubscribe(const std::string &topic, const std::string
 
     std::string client_name = "topic_unsubscriber" + topic;
     auto client = std::make_shared<WsClient>(server_port_path);
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     client->on_open = [client_name, message](std::shared_ptr<WsClient::Connection> connection) {
 #else
     client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
 #endif
 
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         std::cout << client_name << ": Opened connection" << std::endl;
         std::cout << client_name << ": Sending message: " << message << std::endl;
 #endif
@@ -729,7 +708,7 @@ void RosbridgeWsClient::advertiseService(const std::string &client_name, const s
             return service == std::get<integral(EntryEnum::ServiceTopicName)>(td);
         });
         if ( it_ser_top != service_topic_list.end()){
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
             std::cerr << "service: " << service << " already advertised" << std::endl;
 #endif
             return;
@@ -743,7 +722,7 @@ void RosbridgeWsClient::advertiseService(const std::string &client_name, const s
         it_client->second->on_message = callback;
         start(client_name, it_client->second, message);
     }
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     else
     {
         std::cerr << client_name << "has not been created" << std::endl;
@@ -759,7 +738,7 @@ void RosbridgeWsClient::unadvertiseService(const std::string &service){
         return service == std::get<integral(EntryEnum::ServiceTopicName)>(td);
     });
     if ( it_ser_top == service_topic_list.end()){
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         std::cerr << "service: " << service << " not advertised" << std::endl;
 #endif
         return;
@@ -771,13 +750,13 @@ void RosbridgeWsClient::unadvertiseService(const std::string &service){
 
     std::string client_name = "service_unadvertiser" + service;
     auto client = std::make_shared<WsClient>(server_port_path);
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     client->on_open = [client_name, message](std::shared_ptr<WsClient::Connection> connection) {
 #else
     client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
 #endif
 
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         std::cout << client_name << ": Opened connection" << std::endl;
         std::cout << client_name << ": Sending message: " << message << std::endl;
 #endif
@@ -816,12 +795,12 @@ void RosbridgeWsClient::serviceResponse(const std::string &service, const std::s
     std::string client_name = "service_response_client" + service;
     std::shared_ptr<WsClient> service_response_client = std::make_shared<WsClient>(server_port_path);
 
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     service_response_client->on_open = [message, client_name](std::shared_ptr<WsClient::Connection> connection) {
 #else
     service_response_client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
 #endif
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
         std::cout << client_name << ": Opened connection" << std::endl;
         std::cout << client_name << ": Sending message: " << message << std::endl;
 #endif
@@ -869,7 +848,7 @@ void RosbridgeWsClient::callService(const std::string &service, const InMessage
     else
     {
         call_service_client->on_message = [client_name](std::shared_ptr<WsClient::Connection> connection, std::shared_ptr<WsClient::InMessage> in_message) {
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
             std::cout << client_name << ": Message received: " << in_message->string() << std::endl;
             std::cout << client_name << ": Sending close connection" << std::endl;
 #else
@@ -884,7 +863,7 @@ void RosbridgeWsClient::callService(const std::string &service, const InMessage
 
 bool RosbridgeWsClient::serviceAvailable(const std::string &service)
 {
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     std::cout << "checking if service " << service << " is available" << std::endl;
 #endif
     size_t pos;
@@ -892,7 +871,16 @@ bool RosbridgeWsClient::serviceAvailable(const std::string &service)
         std::lock_guard<std::mutex> lk(mutex);
         pos = available_services.find(service);
     }
-    return pos != std::string::npos ? true : false;
+    bool ret = pos != std::string::npos ? true : false;
+#ifdef ROS_BRIDGE_DEBUG
+    if ( ret ){
+        std::cout << "service " << service << " is available" << std::endl;
+    } else {
+        std::cout << "service " << service << " is not available" << std::endl;
+
+    }
+#endif
+    return ret;
 }
 
 void RosbridgeWsClient::waitForService(const std::string &service)
@@ -904,28 +892,27 @@ void RosbridgeWsClient::waitForService(const std::string &service)
 
 void RosbridgeWsClient::waitForService(const std::string &service, const std::function<bool(void)> stop)
 {
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     auto s = std::chrono::high_resolution_clock::now();
     long counter = 0;
 #endif
     const auto poll_interval = std::chrono::milliseconds(1000);
-    auto poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval;
+    auto poll_time_point = std::chrono::high_resolution_clock::now();
     while( !stop() )
     {
         if ( std::chrono::high_resolution_clock::now() > poll_time_point ){
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
             ++counter;
 #endif
             if ( serviceAvailable(service) ){
                 break;
-            } else {
-                poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval;
             }
+            poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval;
         } else {
             std::this_thread::sleep_for(std::chrono::milliseconds(1));
         }
     };
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     auto e = std::chrono::high_resolution_clock::now();
     std::cout << "waitForService() " << service << " time: "
               << std::chrono::duration_cast<std::chrono::milliseconds>(e-s).count()
@@ -944,28 +931,27 @@ void RosbridgeWsClient::waitForTopic(const std::string &topic)
 
 void RosbridgeWsClient::waitForTopic(const std::string &topic, const std::function<bool(void)> stop)
 {
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     auto s = std::chrono::high_resolution_clock::now();
     long counter = 0;
 #endif
     const auto poll_interval = std::chrono::milliseconds(1000);
-    auto poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval;
+    auto poll_time_point = std::chrono::high_resolution_clock::now();
     while( !stop() )
     {
         if ( std::chrono::high_resolution_clock::now() > poll_time_point ){
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
             ++counter;
 #endif
             if ( topicAvailable(topic) ){
                 break;
-            } else {
-                poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval;
             }
+            poll_time_point = std::chrono::high_resolution_clock::now() + poll_interval;
         } else {
             std::this_thread::sleep_for(std::chrono::milliseconds(1));
         }
     };
-#ifdef DEBUG
+#ifdef ROS_BRIDGE_DEBUG
     auto e = std::chrono::high_resolution_clock::now();
     std::cout << "waitForTopic() " << topic << " time: "
               << std::chrono::duration_cast<std::chrono::milliseconds>(e-s).count()

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"
-
-#include <unordered_map>
-
-ROSBridge::ComPrivate::TopicPublisher::TopicPublisher(CasePacker &casePacker,
-                                                      RosbridgeWsClient &rbc) :
-      _stopped(std::make_shared<std::atomic_bool>(true))
-    , _casePacker(casePacker)
-    , _rbc(rbc)
-{
-
-}
-
-ROSBridge::ComPrivate::TopicPublisher::~TopicPublisher()
-{
-    this->reset();
-}
-
-void ROSBridge::ComPrivate::TopicPublisher::start()
-{
-    if ( !_stopped->load() )  // start called while thread running.
-        return;
-    _stopped->store(false);
-    _pThread = std::make_unique<std::thread>([this]{
-        // Init.
-        std::unordered_map<std::string, std::string> topicMap;
-        // Main Loop.
-        while( !this->_stopped->load() ){
-            JsonDocUPtr pJsonDoc;
-            {
-                std::unique_lock<std::mutex> lk(this->_mutex);
-                // Check if new data available, wait if not.
-                if (this->_queue.empty()){
-                    if ( _stopped->load() ) // Check condition again while holding the lock.
-                        break;
-                    this->_cv.wait(lk); // Wait for condition, spurious wakeups don't matter in this case.
-                    continue;
-                }
-                // Pop message from queue.
-                pJsonDoc = std::move(this->_queue.front());
-                this->_queue.pop_front();
-            }
-
-            // Get tag from Json message and remove it.
-            Tag tag;
-            bool ret = this->_casePacker.getTag(pJsonDoc, tag);
-            assert(ret); // Json message does not contain a tag;
-            (void)ret;
-            this->_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();
-            auto it =  topicMap.find(clientName);
-            if ( it ==  topicMap.end()) { // Need to advertise topic?
-                 topicMap.insert(std::make_pair(clientName, tag.topic()));
-                 this->_rbc.addClient(clientName);
-                 this->_rbc.advertise(clientName,
-                                   tag.topic(),
-                                   tag.messageType() );
-                 this->_rbc.waitForTopic(tag.topic(), [this]{
-                     return this->_stopped->load();
-                 }); // Wait until topic is advertised.
-            }
-
-            // Publish Json message.
-            if ( !this->_stopped->load() )
-                this->_rbc.publish(tag.topic(), *pJsonDoc.get());
-        } // while loop
-
-        // Tidy up.
-        for (auto& it :  topicMap){
-            this->_rbc.unadvertise(it.second);
-            this->_rbc.removeClient(it.first);
-        }
-
-        std::cout << "TopicPublisher: publisher thread terminated." << std::endl;
-    });
-}
-
-void ROSBridge::ComPrivate::TopicPublisher::reset()
-{
-    if ( _stopped->load() )  // stop called while thread not running.
-        return;
-    {
-        std::lock_guard<std::mutex> lk(_mutex);
-        std::cout << "TopicPublisher: _stopped->store(true)." << std::endl;
-        _stopped->store(true);
-        std::cout << "TopicPublisher: ask publisher thread to stop." << std::endl;
-        _cv.notify_one(); // Wake publisher thread.
-    }
-    if ( !_pThread )
-        return;
-    _pThread->join();
-    std::cout << "TopicPublisher: publisher thread joined." << std::endl;
-    {
-        _queue.clear();
-        std::cout << "TopicPublisher: queue cleard." << std::endl;
-    }
-}
-

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;
@@ -26,5 +24,11 @@ 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/point32.h

+#pragma once
+
+#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 geometry_msgs {
+//! @brief Namespace containing methodes for geometry_msgs/Point32 message generation.
+namespace point32 {
+
+
+std::string messageType();
+
+namespace detail {
+using namespace ros_bridge::traits;
+
+template <class T>
+auto getZ(const T &p, Type2Type<Has3Components>)
+{
+    return p.z();
+}
+
+template <class T>
+auto getZ(const T &p, Type2Type<Has2Components>)
+{
+    (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 <class T>
+bool setZ(const rapidjson::Value &doc, const T &p, Type2Type<Has2Components>)
+{
+    (void)doc;
+    (void)p;
+    return true;
+}
+
+} // namespace detail
+
+//! @brief C++ representation of a geometry_msgs/Point/Point32
+template<typename FloatType = _Float64, class OStream = std::ostream>
+class GenericPoint {
+public:
+    GenericPoint(FloatType x, FloatType y, FloatType z) : _x(x), _y(y), _z(z){}
+
+    FloatType x() const {return _x;}
+    FloatType y() const {return _y;}
+    FloatType z() const {return _z;}
+
+    void setX(FloatType x) {_x = x;}
+    void setY(FloatType y) {_y = y;}
+    void setZ(FloatType z) {_z = z;}
+
+
+
+    bool operator==(const GenericPoint &p1) {
+        return (p1._x == this->_x
+                && p1._y == this->_y
+                && p1._z == this->_z);
+    }
+
+    bool operator!=(const GenericPoint &p1) {
+        return !this->operator==(p1);
+    }
+
+    friend OStream& operator<<(OStream& os, const GenericPoint& p)
+    {
+        os << "x: " << p._x << " y: " << p._y<< " z: " << p._z;
+        return os;
+    }
+
+private:
+    FloatType _x;
+    FloatType _y;
+    FloatType _z;
+};
+
+typedef GenericPoint<> Point;
+typedef GenericPoint<_Float32> Point32;
+
+
+
+template <class T>
+bool toJson(const T&p, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
+{
+    using namespace ros_bridge::traits;
+
+    value.AddMember("x", rapidjson::Value().SetFloat(p.x()), allocator);
+    value.AddMember("y", rapidjson::Value().SetFloat(p.y()), allocator);
+
+    typedef typename Select<HasMemberZ<T>::value, Has3Components, Has2Components>::Result
+            Components; // Check if PointType has 2 or 3 dimensions.
+    auto z = detail::getZ(p, Type2Type<Components>()); // If T has no member z() replace it by 0.
+
+    value.AddMember("z", rapidjson::Value().SetFloat(z), allocator);
+    return true;
+}
+
+template <class PointType>
+bool fromJson(const rapidjson::Value &value,
+              PointType &p)     {
+    using namespace ros_bridge::traits;
+
+    if (!value.HasMember("x") || !value["x"].IsFloat()){
+        assert(false);
+        return false;
+    }
+    if (!value.HasMember("y") || !value["y"].IsFloat()){
+        assert(false);
+        return false;
+    }
+    if (!value.HasMember("z") || !value["z"].IsFloat()){
+        assert(false);
+        return false;
+    }
+
+    p.setX(value["x"].GetFloat());
+    p.setY(value["y"].GetFloat());
+    typedef typename Select<HasMemberSetZ<PointType>::value, Has3Components, Has2Components>::Result
+            Components; // Check if PointType has 2 or 3 dimensions.
+    (void)detail::setZ(value["z"], p,  Type2Type<Components>()); // If PointType has no member z() discard doc["z"].
+
+    return true;
+}
+
+} // namespace point32
+} // namespace geometry_msgs
+} // namespace messages
+} // namespace ros_bridge

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

+#pragma once
+#include "ros_bridge/rapidjson/include/rapidjson/document.h"
+#include "ros_bridge/include/messages/geometry_msgs/point32.h"
+
+#include <type_traits>
+#include <vector>
+
+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 geometry_msgs {
+//! @brief Namespace containing methodes for geometry_msgs/Polygon message generation.
+namespace polygon {
+
+
+std::string messageType();
+
+//! @brief C++ representation of geometry_msgs/Polygon
+template <class PointTypeCVR,
+          template <class, class...> class ContainerType = std::vector>
+class GenericPolygon {
+    typedef typename std::remove_cv_t<typename std::remove_reference_t<PointTypeCVR>> PointType;
+public:
+    GenericPolygon() {}
+    GenericPolygon(const GenericPolygon &poly) : _points(poly.points()){}
+
+
+    const ContainerType<PointType> &points() const {return _points;}
+          ContainerType<PointType> &points()       {return _points;}
+
+          GenericPolygon &operator=(const GenericPolygon &other) {
+              this->_points = other._points;
+              return *this;
+          }
+
+private:
+    ContainerType<PointType> _points;
+};
+typedef GenericPolygon<geometry_msgs::point32::Point> Polygon;
+
+
+template <class PolygonType>
+bool toJson(const PolygonType &poly, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
+{
+    using namespace geometry_msgs::point32;
+
+    rapidjson::Value points(rapidjson::kArrayType);
+
+    for(unsigned long i=0; i < std::uint64_t(poly.points().size()); ++i) {
+        rapidjson::Document point(rapidjson::kObjectType);
+        if ( !point32::toJson(poly.points()[i], point, allocator) ){
+            assert(false);
+            return false;
+        }
+        points.PushBack(point, allocator);
+    }
+
+    value.AddMember("points", points, allocator);
+    return true;
+}
+
+template <class PolygonType>
+bool fromJson(const rapidjson::Value &value, PolygonType &poly)
+{
+    using namespace geometry_msgs::point32;
+
+    if (!value.HasMember("points") || !value["points"].IsArray()){
+        assert(false);
+        return false;
+    }
+    const auto &jsonArray = value["points"].GetArray();
+    poly.points().clear();
+    poly.points().reserve(jsonArray.Size());
+    typedef decltype (poly.points()[0]) PointTypeCVR;
+    typedef typename std::remove_cv_t<typename std::remove_reference_t<PointTypeCVR>> PointType;
+    for (long i=0; i < jsonArray.Size(); ++i){
+        PointType pt;
+        if ( !point32::fromJson(jsonArray[i], pt) ){
+            assert(false);
+            return false;
+        }
+        poly.points().push_back(std::move(pt));
+    }
+    return true;
+}
+
+} // namespace polygon
+} // namespace geometry_msgs
+} // namespace messages
+} // namespace ros_bridge

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/geometry_msgs/polygon_stamped.h

+#pragma once
+
+#include "ros_bridge/rapidjson/include/rapidjson/document.h"
+#include "ros_bridge/include/messages/geometry_msgs/polygon.h"
+#include "ros_bridge/include/messages/std_msgs/header.h"
+#include "ros_bridge/include/message_traits.h"
+
+#include <type_traits>
+
+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 geometry_msgs {
+//! @brief Namespace containing methodes for geometry_msgs/PolygonStamped message generation.
+namespace polygon_stamped {
+
+
+std::string messageType();
+
+//! @brief C++ representation of geometry_msgs/PolygonStamped
+template <class PolygonType  = geometry_msgs::polygon::Polygon,
+          class HeaderType = std_msgs::header::Header>
+class GenericPolygonStamped {
+    typedef PolygonType Polygon;
+public:
+    GenericPolygonStamped(){}
+    GenericPolygonStamped(const GenericPolygonStamped &other) :
+        _header(other.header())
+      , _polygon(other.polygon())
+    {}
+    GenericPolygonStamped(const HeaderType &header, Polygon &polygon) :
+        _header(header)
+      , _polygon(polygon)
+    {}
+
+
+    const HeaderType  &header()  const   {return _header;}
+    const Polygon     &polygon() const   {return _polygon;}
+
+    HeaderType  &header()  {return _header;}
+    Polygon     &polygon() {return _polygon;}
+
+private:
+    HeaderType   _header;
+    Polygon      _polygon;
+};
+
+typedef GenericPolygonStamped<> PolygonStamped;
+
+// ===================================================================================
+namespace detail {
+
+template <class PolygonStampedType>
+bool setHeader(const rapidjson::Value &doc,
+               PolygonStampedType &polyStamped,
+               traits::Int2Type<1>) { // polyStamped.header() exists
+    using namespace std_msgs;
+
+    typedef decltype (polyStamped.header()) HeaderTypeCVR;
+    typedef typename std::remove_cv_t<typename std::remove_reference_t<HeaderTypeCVR>> HeaderType;
+    HeaderType header;
+    bool ret = header::fromJson(doc, header);
+    polyStamped.header() = header;
+    return ret;
+}
+template <class PolygonStampedType>
+bool setHeader(const rapidjson::Value &doc,
+               PolygonStampedType &polyStamped,
+               traits::Int2Type<0>) { // polyStamped.header() does not exists
+    (void)doc;
+    (void)polyStamped;
+    return true;
+}
+
+
+template <class PolygonType, class HeaderType>
+bool _toJson(const PolygonType &poly,
+             const HeaderType &h,
+             rapidjson::Value &value,
+             rapidjson::Document::AllocatorType &allocator)
+{
+    using namespace std_msgs;
+    using namespace geometry_msgs;
+
+    rapidjson::Document header(rapidjson::kObjectType);
+    if (!header::toJson(h, header, allocator)){
+        assert(false);
+        return false;
+    }
+    rapidjson::Document polygon(rapidjson::kObjectType);
+    if (!polygon::toJson(poly, polygon, allocator)){
+        assert(false);
+        return false;
+    }
+    value.AddMember("header", header, allocator);
+    value.AddMember("polygon", polygon, allocator);
+
+    return true;
+}
+
+template<class PolyStamped, int k>
+bool _toJson(const PolyStamped &polyStamped,
+             rapidjson::Value &value,
+             rapidjson::Document::AllocatorType &allocator,
+             traits::Int2Type<k>){ // PolyStamped has member header(), use integraded header.
+return _toJson(polyStamped, polyStamped.header(), value, allocator);
+}
+
+template<class PolyStamped>
+bool _toJson(const PolyStamped &polyStamped,
+             rapidjson::Value &value,
+             rapidjson::Document::AllocatorType &allocator,
+             traits::Int2Type<0>){ // PolyStamped has no member header(), generate one on the fly.
+using namespace std_msgs::header;
+return _toJson(polyStamped, Header(), value, allocator);
+}
+
+} // namespace detail
+// ===================================================================================
+
+
+template <class PolygonType, class HeaderType>
+bool toJson(const PolygonType &poly,
+            const HeaderType &h,
+            rapidjson::Value &value,
+            rapidjson::Document::AllocatorType &allocator)
+{
+    return detail::_toJson(poly, h, value, allocator);
+}
+
+template <class PolyStamped>
+bool toJson(const PolyStamped &polyStamped,
+            rapidjson::Value &value,
+            rapidjson::Document::AllocatorType &allocator)
+{
+    typedef traits::HasMemberHeader<PolyStamped> HasHeader;
+    return detail::_toJson(polyStamped, value, allocator, traits::Int2Type<HasHeader::value>());
+}
+
+template <class PolygonType, class HeaderType>
+bool fromJson(const rapidjson::Value &value, PolygonType &polyStamped)
+{
+    using namespace geometry_msgs::polygon;
+
+    if ( !value.HasMember("header") ){
+        assert(false);
+        return false;
+    }
+    if ( !value.HasMember("polygon") ){
+        assert(false);
+        return false;
+    }
+
+    typedef traits::HasMemberSetHeader<PolygonType> HasHeader;
+    if ( !detail::setHeader(value["header"], polyStamped, traits::Int2Type<HasHeader::value>())){
+        assert(false);
+        return false;
+    }
+
+    if ( !polygon::fromJson(value["polygon"], polyStamped.polygon()) ){
+        assert(false);
+        return false;
+    }
+
+    return true;
+}
+
+} // namespace polygon_stamped
+} // namespace geometry_msgs
+} // namespace messages
+} // namespace ros_bridge

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/jsk_recognition_msgs/polygon_array.h

+#pragma once
+#include "ros_bridge/rapidjson/include/rapidjson/document.h"
+#include "ros_bridge/include/message_traits.h"
+#include "ros_bridge/include/messages/geometry_msgs/polygon_stamped.h"
+#include "ros_bridge/include/messages/std_msgs/header.h"
+
+#include <type_traits>
+#include <vector>
+
+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 jsk_recognition_msgs {
+//! @brief Namespace containing methodes for jsk_recognition_msgs/PolygonArray message generation.
+namespace polygon_array {
+
+
+std::string messageType();
+
+
+//! @brief C++ representation of jsk_recognition_msgs/PolygonArray
+template <class PolygonType = geometry_msgs::polygon_stamped::PolygonStamped,
+          template <class, class...> class ContainerType = std::vector,
+          class HeaderType =  std_msgs::header::Header,
+          class IntType = long,
+          class FloatType = double>
+class GenericPolygonArray{
+public:
+    GenericPolygonArray() {}
+    GenericPolygonArray(const GenericPolygonArray &other)
+      : _header(other.header())
+      , _polygons(other.polygons())
+      , _labels(other.labels())
+      , _likelihood(other.likelihood())
+    {}
+    GenericPolygonArray(const HeaderType                   &header,
+                 const ContainerType<PolygonType>   &polygons,
+                 const ContainerType<IntType>      &labels,
+                 const ContainerType<FloatType>      &likelihood)
+      : _header(header)
+      , _polygons(polygons)
+      , _labels(labels)
+      , _likelihood(likelihood)
+    {}
+
+    const HeaderType                 &header()     const {return _header;}
+          HeaderType                 &header()           {return _header;}
+    const ContainerType<PolygonType> &polygons()   const {return _polygons;}
+          ContainerType<PolygonType> &polygons()         {return _polygons;}
+    const ContainerType<IntType>     &labels()     const {return _labels;}
+          ContainerType<IntType>     &labels()           {return _labels;}
+    const ContainerType<FloatType>   &likelyhood() const {return _likelihood;}
+          ContainerType<FloatType>   &likelyhood()       {return _likelihood;}
+
+
+private:
+    HeaderType                  _header;
+    ContainerType<PolygonType>  _polygons;
+    ContainerType<IntType>      _labels;
+    ContainerType<FloatType>    _likelihood;
+};
+typedef GenericPolygonArray<> PolygonArray;
+
+
+namespace detail {
+    //! Helper functions to generate Json entries for labels and likelihood.
+
+    //! \note \p p has member \fn labels().
+    template <class PolygonArrayType, int  k>
+    void labelsToJson(const PolygonArrayType &p,
+                      rapidjson::Value &labels,
+                      rapidjson::Document::AllocatorType &allocator,
+                      traits::Int2Type<k>){
+        for(unsigned long i=0; i < (unsigned long)p.labels().size(); ++i)
+            labels.PushBack(rapidjson::Value().SetUint(p.labels()[i]), allocator);
+    }
+
+    //! \note \p p has no member \fn labels().
+    template <class PolygonArrayType>
+    void labelsToJson(const PolygonArrayType &p,
+                      rapidjson::Value &labels,
+                      rapidjson::Document::AllocatorType &allocator,
+                      traits::Int2Type<0>){
+        for(unsigned long i=0; i < (unsigned long)(p.polygons().size()); ++i)
+            labels.PushBack(rapidjson::Value().SetUint(0), allocator); // use zero!
+    }
+
+    //! \note \p p has member \fn likelihood().
+    template <class PolygonArrayType, int  k>
+    void likelihoodToJson(const PolygonArrayType &p,
+                          rapidjson::Value &likelyhood,
+                          rapidjson::Document::AllocatorType &allocator,
+                          traits::Int2Type<k>){
+        p.likelyhood().clear();
+        for(unsigned long i=0; i < (unsigned long)p.likelyhood().size(); ++i)
+            likelyhood.PushBack(rapidjson::Value().SetFloat(p.likelyhood()[i]), allocator);
+    }
+
+    //! \note \p p has no member \fn likelihood().
+    template <class PolygonArrayType>
+    void likelihoodToJson(const PolygonArrayType &p,
+                          rapidjson::Value &likelyhood,
+                          rapidjson::Document::AllocatorType &allocator,
+                          traits::Int2Type<0>){
+        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,
+                   traits::Int2Type<k>){
+        p.labels().clear();
+        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,
+                   traits::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,
+                       traits::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,
+                       traits::Int2Type<0>){
+        (void)doc;
+        (void)p;
+    }
+
+    template <class PolygonArrayType, class HeaderType>
+    bool _toJson(const PolygonArrayType &p,
+                 const HeaderType &h,
+                 rapidjson::Value &value,
+                 rapidjson::Document::AllocatorType &allocator)
+    {
+        using namespace std_msgs;
+        using namespace geometry_msgs;
+
+        rapidjson::Document jHeader(rapidjson::kObjectType);
+        if (!header::toJson(h, jHeader, allocator)){
+            assert(false);
+            return false;
+        }
+        value.AddMember("header", jHeader, allocator);
+
+        rapidjson::Value jPolygons(rapidjson::kArrayType);
+        for(unsigned long i=0; i < (unsigned long)(p.polygons().size()); ++i){
+            rapidjson::Document jPolygon(rapidjson::kObjectType);
+            if (!polygon_stamped::toJson(p.polygons()[i].polygon(), h, jPolygon, allocator)){
+                assert(false);
+                return false;
+            }
+            jPolygons.PushBack(jPolygon, allocator);
+        }
+        value.AddMember("polygons", jPolygons, allocator);
+
+
+        rapidjson::Value jLabels(rapidjson::kArrayType);
+        typedef traits::HasMemberLabels<PolygonArrayType> HasLabels;
+        detail::labelsToJson(p, jLabels, allocator, traits::Int2Type<HasLabels::value>());
+        value.AddMember("labels", jLabels, allocator);
+
+
+        rapidjson::Value jLikelihood(rapidjson::kArrayType);
+        typedef traits::HasMemberLikelihood<PolygonArrayType> HasLikelihood;
+        detail::likelihoodToJson(p, jLikelihood, allocator, traits::Int2Type<HasLikelihood::value>());
+        value.AddMember("likelihood", jLikelihood, allocator);
+
+        return true;
+    }
+
+    template<class PolygonArrayType, int k>
+    bool _toJson(const PolygonArrayType &p,
+                 rapidjson::Value &value,
+                 rapidjson::Document::AllocatorType &allocator,
+                 traits::Int2Type<k>)
+    {
+        // U has member header(), use integraded header.
+        return _toJson(p, p.header(), value, allocator);
+    }
+
+    template<class PolygonArrayType>
+    bool _toJson(const PolygonArrayType &p,
+                 rapidjson::Value &value,
+                 rapidjson::Document::AllocatorType &allocator,
+                 traits::Int2Type<0>)
+    {
+        // U has no member header(), generate one on the fly.
+        using namespace std_msgs::header;
+        return _toJson(p, Header(), value, allocator);
+    }
+}
+
+
+//!
+//! Create PolygonArray message from \p p and \p h. \p p doesn't have it's own header.
+//! \param p Class implementing the PolygonArray interface.
+//! \param h Class implementing the Header interface.
+//! \param doc Rapidjson document used to store the PolygonArray message.
+//! \param allocator Allocator used by doc. Can be obtained e.g. by calling doc.getAllocator().
+//!
+//! \note The \fn labels() and \fn likelihood() members are optinal. If any of them is missing they
+//! will be replaced by arrays filled with zero and size p.polygons.size().
+//!
+//! \note If this function is called, the headers in p.polygons[i] (entries implement the the PolygonStamped interface)
+//! are ignored.
+template <class PolygonArrayType, class HeaderType>
+bool toJson(const PolygonArrayType &p,
+            const HeaderType &h,
+            rapidjson::Value &value,
+            rapidjson::Document::AllocatorType &allocator)
+{
+    return detail::_toJson(p, h, value, allocator);
+}
+
+
+
+//!
+//! Create PolygonArray message from \p p. \p p contains a header.
+//! \param p Class implementing the PolygonArrayType interface.
+//! \param doc Rapidjson document used to store the PolygonArray message.
+//! \param allocator Allocator used by doc. Can be obtained e.g. by calling doc.getAllocator().
+//!
+//! \note The \fn labels() and \fn likelihood() members are optinal. If any of them is missing they
+//! will be replaced by arrays filled with zero and size p.polygons.size().
+//!
+//! \note If the header() function is missing, a default constructed header is used.
+template <class PolygonArrayType>
+bool toJson(const PolygonArrayType &p,
+            rapidjson::Value &value,
+            rapidjson::Document::AllocatorType &allocator)
+{
+    typedef traits::HasMemberHeader<PolygonArrayType> HasHeader;
+    return detail::_toJson(p, value, allocator, traits::Int2Type<HasHeader::value>());
+}
+
+
+
+template <class PolygonArrayType>
+bool fromJson(const rapidjson::Value &value,
+              PolygonArrayType &p)
+{
+    using namespace geometry_msgs;
+    if ( !value.HasMember("header")){
+        assert(false);
+        return false;
+    }
+    if ( !value.HasMember("polygons") || !value["polygons"].IsArray() ){
+        assert(false);
+        return false;
+    }
+    if ( !value.HasMember("labels") || !value["labels"].IsArray() ){
+        assert(false);
+        return false;
+    }
+    if ( !value.HasMember("likelihood") || !value["likelihood"].IsArray() ){
+        assert(false);
+        return false;
+    }
+
+
+    typedef traits::HasMemberHeader<PolygonArrayType> HasHeader;
+    if ( !polygon_stamped::detail::setHeader(value["header"],
+                                             p,
+                                             traits::Int2Type<HasHeader::value>())){
+        assert(false);
+        return false;
+    }
+
+    const auto &jPolygonStamped = value["polygons"];
+    p.polygons().clear();
+    p.polygons().reserve(jPolygonStamped.Size());
+    typedef decltype (p.polygons()[0]) PolyStampedCVR;
+    typedef typename std::remove_cv_t<
+            typename std::remove_reference_t<PolyStampedCVR>>
+            PolyStamped;
+    for (unsigned int i=0; i < jPolygonStamped.Size(); ++i) {
+        if ( !jPolygonStamped[i].HasMember("header") ){
+            assert(false);
+            return false;
+        }
+        PolyStamped polygonStamped;
+
+        if ( !polygon_stamped::detail::setHeader(jPolygonStamped[i]["header"],
+                                                 polygonStamped,
+                                                 traits::Int2Type<HasHeader::value>())){
+            assert(false);
+            return false;
+        }
+
+        if ( !polygon::fromJson(jPolygonStamped[i]["polygon"], polygonStamped.polygon())){
+            assert(false);
+            return false;
+        }
+        p.polygons().push_back(std::move(polygonStamped));
+    }
+
+    typedef traits::HasMemberLabels<PolygonArrayType> HasLabels;
+    detail::setLabels(value["labels"], p, traits::Int2Type<HasLabels::value>());
+
+    typedef traits::HasMemberLikelihood<PolygonArrayType> HasLikelihood;
+    detail::setLikelihood(value["likelihood"], p, traits::Int2Type<HasLikelihood::value>());
+
+    return true;
+}
+
+
+} // namespace polygon_array
+} // namespace geometry_msgs
+} // namespace messages
+} // namespace ros_bridge

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

+#pragma once
+
+#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 nemo_msgs {
+//! @brief Namespace containing methodes for geometry_msgs/GeoPoint message generation.
+namespace heartbeat {
+
+
+std::string messageType();
+
+
+//! @brief C++ representation of nemo_msgs/Heartbeat message
+class Heartbeat{
+public:
+    Heartbeat(){}
+    Heartbeat(int status) :_status(status){}
+    Heartbeat(const Heartbeat &heartbeat) : _status(heartbeat.status()){}
+
+    virtual int     status      (void)const    {return _status;}
+    virtual void    setStatus   (int status)    {_status = status;}
+protected:
+    int _status;
+};
+
+
+template <class HeartbeatType>
+bool toJson(const HeartbeatType &heartbeat, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
+{
+    value.AddMember("status", std::int32_t(heartbeat.status()), allocator);
+    return true;
+}
+
+template <class HeartbeatType>
+bool fromJson(const rapidjson::Value &value, HeartbeatType &heartbeat)
+{
+    if (!value.HasMember("status") || !value["status"].IsInt()){
+        assert(false);
+        return false;
+    }
+
+    heartbeat.setStatus(value["status"].GetInt());
+    return true;
+}
+
+} // namespace heartbeat
+} // namespace nemo_msgs
+} // namespace messages
+} // namespace ros_bridge

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/nemo_msgs/progress.h

+#pragma once
+
+#include "ros_bridge/rapidjson/include/rapidjson/document.h"
+
+#include <vector>
+
+
+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 nemo_msgs {
+//! @brief Namespace containing methodes for geometry_msgs/Point32 message generation.
+namespace progress {
+
+
+std::string messageType();
+
+//! @brief C++ representation of nemo_msgs/Progress message
+template <class IntType = long, template <class, class...> class ContainterType = std::vector>
+class GenericProgress{
+public:
+    GenericProgress() {}
+    GenericProgress(const ContainterType<IntType> &progress) : _progress(progress){}
+    GenericProgress(const GenericProgress &p) : _progress(p.progress()){}
+
+    virtual const ContainterType<IntType> &progress(void) const {return _progress;}
+    virtual       ContainterType<IntType> &progress(void)       {return _progress;}
+
+protected:
+    ContainterType<IntType> _progress;
+};
+typedef GenericProgress<> Progress;
+
+
+template <class ProgressType>
+bool toJson(const ProgressType &p, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
+{
+    rapidjson::Value jProgress(rapidjson::kArrayType);
+    for(unsigned long i=0; i < std::uint64_t(p.progress().size()); ++i){
+        jProgress.PushBack(rapidjson::Value().SetInt(std::int32_t(p.progress()[i])), allocator);
+    }
+    value.AddMember("progress", jProgress, allocator);
+    return true;
+}
+
+template <class ProgressType>
+bool fromJson(const rapidjson::Value &value, ProgressType &p)
+{
+    if (!value.HasMember("progress") || !value["progress"].IsArray()){
+        assert(false);
+        return false;
+    }
+
+    const auto& jsonProgress = value["progress"];
+    unsigned long sz = jsonProgress.Size();
+    p.progress().clear();
+    p.progress().reserve(sz);
+    for (unsigned long i=0; i < sz; ++i)
+        p.progress().push_back(std::int32_t(jsonProgress[i].GetInt()));
+    return true;
+}
+
+} // namespace progress
+} // namespace nemo_msgs
+} // namespace messages
+} // namespace ros_bridge

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

+#include "header.h"
+
+std::uint32_t ros_bridge::messages::std_msgs::header::Header::_defaultSeq = 0;
+
+ros_bridge::messages::std_msgs::header::Header::Header()
+    : _seq(_defaultSeq++),
+      _stamp(Time()),
+      _frameId("")
+{}
+
+ros_bridge::messages::std_msgs::header::Header::Header(
+        uint32_t seq,
+        const ros_bridge::messages::std_msgs::header::Header::Time &stamp,
+        const std::string &frame_id)
+    : _seq(seq)
+    , _stamp(stamp)
+    , _frameId(frame_id) {}
+
+ros_bridge::messages::std_msgs::header::Header::Header(
+        const ros_bridge::messages::std_msgs::header::Header &header)
+    : _seq(header.seq())
+    , _stamp(header.stamp())
+    , _frameId(header.frameId()) {}
+
+uint32_t ros_bridge::messages::std_msgs::header::Header::seq() const
+{
+    return _seq;
+}
+
+const ros_bridge::messages::std_msgs::header::Header::Time &ros_bridge::messages::std_msgs::header::Header::stamp() const
+{
+    return _stamp;
+}
+
+const std::string &ros_bridge::messages::std_msgs::header::Header::frameId() const
+{
+    return _frameId;
+}
+
+ros_bridge::messages::std_msgs::header::Header::Time &ros_bridge::messages::std_msgs::header::Header::stamp()
+{
+    return _stamp;
+}
+
+std::string &ros_bridge::messages::std_msgs::header::Header::frameId()
+{
+    return _frameId;
+}
+
+void ros_bridge::messages::std_msgs::header::Header::setSeq(uint32_t seq)
+{
+    _seq = seq;
+}
+
+void ros_bridge::messages::std_msgs::header::Header::setStamp(
+        const ros_bridge::messages::std_msgs::header::Header::Time &stamp)
+{
+    _stamp = stamp;
+}
+
+void ros_bridge::messages::std_msgs::header::Header::setFrameId(
+        const std::string &frameId)
+{
+    _frameId = frameId;
+}
+
+std::string ros_bridge::messages::std_msgs::header::messageType(){
+    return "std_msgs/Header";
+}

src/comm/ros_bridge/include/messages/std_msgs/header.h

+#pragma once
+
+#include "ros_bridge/rapidjson/include/rapidjson/document.h"
+#include "ros_bridge/include/messages/std_msgs/time.h"
+
+#include <string>
+
+namespace ros_bridge {
+//! @brief Namespace containing classes and methodes ros message generation.
+namespace messages {
+//! @brief Namespace containing classes and methodes for std_msgs generation.
+namespace std_msgs {
+//! @brief Namespace containing classes and methodes for std_msgs/Header message generation.
+namespace header {
+
+
+std::string messageType();
+
+//! @brief C++ representation of std_msgs/Header
+class Header{
+public:
+    using Time = std_msgs::time::Time;
+    Header();
+    Header(uint32_t seq, const Time &stamp, const std::string &frame_id);
+    Header(const Header &header);
+
+    uint32_t            seq()       const;
+    const Time          &stamp()     const;
+    const std::string   &frameId()   const;
+
+    Time            &stamp();
+    std::string     &frameId();
+
+    void setSeq     (uint32_t seq);
+    void setStamp   (const Time &stamp);
+    void setFrameId (const std::string  &frameId);
+private:
+    static uint32_t _defaultSeq;
+    uint32_t    _seq;
+    Time        _stamp;
+    std::string _frameId;
+};
+
+template <class HeaderType>
+bool toJson(const HeaderType &header,
+            rapidjson::Value &value,
+            rapidjson::Document::AllocatorType &allocator)
+{
+    using namespace messages::std_msgs;
+
+    value.AddMember("seq", rapidjson::Value().SetUint(uint32_t(header.seq())), allocator);
+
+    rapidjson::Value stamp(rapidjson::kObjectType);
+    if (!time::toJson(header.stamp(), stamp, allocator)){
+        assert(false);
+        return false;
+    }
+    value.AddMember("stamp", stamp, allocator);
+
+    value.AddMember("frame_id",
+                  rapidjson::Value().SetString(header.frameId().data(),
+                                               header.frameId().length(),
+                                               allocator),
+                  allocator);
+
+    return true;
+}
+
+template <class HeaderType>
+bool fromJson(const rapidjson::Value &value,
+              HeaderType &header)
+{
+    using namespace messages::std_msgs;
+
+    if (!value.HasMember("seq")|| !value["seq"].IsUint()){
+        assert(false);
+        return false;
+    }
+    if (!value.HasMember("stamp")){
+        assert(false);
+        return false;
+    }
+    if (!value.HasMember("frame_id")|| !value["frame_id"].IsString()){
+        assert(false);
+        return false;
+    }
+    header.setSeq(value["seq"].GetUint());
+    decltype(header.stamp()) time;
+    if (!time::fromJson(value["stamp"], time)){
+        assert(false);
+        return false;
+    }
+    header.setStamp(time);
+    header.setFrameId(value["frame_id"].GetString());
+
+    return true;
+}
+
+} // namespace time
+} // namespace std_msgs
+} // namespace messages
+} // namespace ros_bridge

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";
+}

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

+#pragma once
+
+#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 std_msgs generation.
+namespace std_msgs {
+//! @brief Namespace containing classes and methodes for std_msgs/Time message generation.
+namespace time {
+
+std::string messageType();
+
+//! @brief C++ representation of std_msgs/Time
+class Time{
+public:
+    Time(): _secs(0), _nsecs(0) {}
+    Time(uint32_t secs, uint32_t nsecs): _secs(secs), _nsecs(nsecs) {}
+    Time(const Time &time): _secs(time.secs()), _nsecs(time.nSecs()) {}
+
+    uint32_t secs()  const {return _secs;}
+    uint32_t nSecs() const {return _nsecs;}
+
+    void setSecs(uint32_t secs)     {_secs = secs;}
+    void setNSecs(uint32_t nsecs)   {_nsecs = nsecs;}
+
+private:
+    uint32_t _secs;
+    uint32_t _nsecs;
+};
+
+template<class TimeType>
+bool toJson(const TimeType &time,
+            rapidjson::Value &value,
+            rapidjson::Document::AllocatorType &allocator)
+{
+    value.AddMember("secs", rapidjson::Value().SetUint(uint32_t(time.secs())), allocator);
+    value.AddMember("nsecs", rapidjson::Value().SetUint(uint32_t(time.nSecs())), allocator);
+
+    return true;
+}
+
+template<class TimeType>
+bool fromJson(const rapidjson::Value &value,
+              TimeType &time)
+{
+    if (!value.HasMember("secs") || !value["secs"].IsUint()){
+        assert(false);
+        return false;
+    }
+    if (!value.HasMember("nsecs")|| !value["nsecs"].IsUint()){
+        assert(false);
+        return false;
+    }
+    time.setSecs(value["secs"].GetUint());
+    time.setNSecs(value["nsecs"].GetUint());
+
+    return true;
+}
+
+} // namespace time
+} // namespace std_msgs
+} // namespace messages
+} // namespace ros_bridge

src/comm/ros_bridge/include/ROSBridge.h → src/comm/ros_bridge/include/ros_bridge.h

 #pragma once
 
 #include "ros_bridge/rapidjson/include/rapidjson/document.h"
-#include "ros_bridge/include/CasePacker.h"
-#include "ros_bridge/include/TypeFactory.h"
-#include "ros_bridge/include/JsonFactory.h"
-#include "ros_bridge/include/TopicPublisher.h"
-#include "ros_bridge/include/TopicSubscriber.h"
-#include "ros_bridge/include/Server.h"
+#include "ros_bridge/include/topic_publisher.h"
+#include "ros_bridge/include/topic_subscriber.h"
+#include "ros_bridge/include/server.h"
 
 #include <memory>
 #include <functional>
 
-namespace ROSBridge {
+namespace ros_bridge {
 class ROSBridge
 {
 public:
-    typedef MessageTag Tag;
     typedef rapidjson::Document JsonDoc;
     typedef std::unique_ptr<JsonDoc> JsonDocUPtr;
 
     explicit ROSBridge();
     explicit ROSBridge(const char* connectionString);
-
-    template<class T>
-    void publish(T &msg, const std::string &topic){
-        _topicPublisher.publish(msg, topic);
-    }
-    void publish(JsonDocUPtr doc);
+    void publish(JsonDocUPtr doc, const char* topic);
     void subscribe(const char *topic,
                    const std::function<void(JsonDocUPtr)> &callBack);
-    void advertiseService(const std::string &service,
-                          const std::string &type,
+    void advertiseService(const char* service,
+                          const char* type,
                           const std::function<JsonDocUPtr(JsonDocUPtr)> &callback);
-
-    const CasePacker *casePacker() const;
+    void advertiseTopic(const char* topic,
+                        const char* type);
 
     //! @brief Start ROS bridge.
     void start();
     //! @brief Stops ROS bridge.
     void reset();
+
     //! @return Returns true if connected to the rosbridge_server, false either.
-    //! @note This function can block up to 100ms. However normal execution time is smaller.
+    //! @note \fn calls start().
     bool connected();
-
     bool isRunning();
 
 private:
     std::shared_ptr<std::atomic_bool>   _stopped;
-    TypeFactory             _typeFactory;
-    JsonFactory             _jsonFactory;
-    CasePacker              _casePacker;
     RosbridgeWsClient                   _rbc;
-    ComPrivate::TopicPublisher     _topicPublisher;
-    ComPrivate::TopicSubscriber    _topicSubscriber;
-    ComPrivate::Server             _server;
+    com_private::TopicPublisher          _topicPublisher;
+    com_private::TopicSubscriber         _topicSubscriber;
+    com_private::Server                  _server;
 
 };
 

src/comm/ros_bridge/include/Server.cpp → src/comm/ros_bridge/include/server.cpp

-#include "Server.h"
+#include "server.h"
 
 #include "rapidjson/include/rapidjson/ostreamwrapper.h"
 
-ROSBridge::ComPrivate::Server::Server(CasePacker &casePacker, RosbridgeWsClient &rbc) :
+ros_bridge::com_private::Server::Server(RosbridgeWsClient &rbc) :
     _rbc(rbc)
-  , _casePacker(casePacker)
   , _stopped(std::make_shared<std::atomic_bool>(true))
 {
 
 }
 
-void ROSBridge::ComPrivate::Server::advertiseService(const std::string &service,
+void ros_bridge::com_private::Server::advertiseService(const std::string &service,
                                                      const std::string &type,
                                                      const Server::CallbackType &userCallback)
 {
@@ -22,9 +21,8 @@ void ROSBridge::ComPrivate::Server::advertiseService(const std::string &service,
     _rbc.addClient(clientName);
 
     auto rbc = &_rbc;
-    auto casePacker = &_casePacker;
     _rbc.advertiseService(clientName, service, type,
-                          [userCallback, rbc, casePacker](
+                          [userCallback, rbc](
                           std::shared_ptr<WsClient::Connection>,
                           std::shared_ptr<WsClient::InMessage> in_message){
         // message->string() is destructive, so we have to buffer it first
@@ -74,17 +72,17 @@ void ROSBridge::ComPrivate::Server::advertiseService(const std::string &service,
     });
 }
 
-ROSBridge::ComPrivate::Server::~Server()
+ros_bridge::com_private::Server::~Server()
 {
     this->reset();
 }
 
-void ROSBridge::ComPrivate::Server::start()
+void ros_bridge::com_private::Server::start()
 {
     _stopped->store(false);
 }
 
-void ROSBridge::ComPrivate::Server::reset()
+void ros_bridge::com_private::Server::reset()
 {
     if ( _stopped->load() )
         return;

src/comm/ros_bridge/include/Server.h → src/comm/ros_bridge/include/server.h

 #pragma once
 
-#include "ros_bridge/include/ComPrivateInclude.h"
+#include "ros_bridge/include/com_private.h"
 #include "ros_bridge/include/RosBridgeClient.h"
-#include "ros_bridge/include/TypeFactory.h"
-#include "ros_bridge/include/CasePacker.h"
 
 #include <unordered_map>
 
-namespace ROSBridge {
-namespace ComPrivate {
+namespace ros_bridge {
+namespace com_private {
 
 class Server
 {
@@ -18,7 +16,7 @@ public:
 
 
     Server() = delete;
-    Server(CasePacker &casePacker, RosbridgeWsClient &rbc);
+    Server(RosbridgeWsClient &rbc);
     ~Server();
 
     //! @brief Starts the subscriber.
@@ -34,7 +32,6 @@ public:
 private:
 
     RosbridgeWsClient  &_rbc;
-    CasePacker         &_casePacker;
     ServiceMap         _serviceMap;
     std::shared_ptr<std::atomic_bool> _stopped;
 };

src/comm/ros_bridge/include/topic_publisher.cpp

+#include "topic_publisher.h"
+
+#include <unordered_map>
+
+ros_bridge::com_private::TopicPublisher::TopicPublisher(RosbridgeWsClient &rbc) :
+      _stopped(std::make_shared<std::atomic_bool>(true))
+    , _rbc(rbc)
+{
+
+}
+
+ros_bridge::com_private::TopicPublisher::~TopicPublisher()
+{
+    this->reset();
+}
+
+void ros_bridge::com_private::TopicPublisher::start()
+{
+    if ( !_stopped->load() )  // start called while thread running.
+        return;
+    _stopped->store(false);
+    _pThread = std::make_unique<std::thread>([this]{
+        // Main Loop.
+        while( !this->_stopped->load() ){
+            std::unique_lock<std::mutex> lk(this->_mutex);
+            // Check if new data available, wait if not.
+            if (this->_queue.empty()){
+                if ( _stopped->load() ) // Check condition again while holding the lock.
+                    break;
+                this->_cv.wait(lk); // Wait for condition, spurious wakeups don't matter in this case.
+                continue;
+            }
+            // Pop message from queue.
+            JsonDocUPtr pJsonDoc = std::move(this->_queue.front());
+            this->_queue.pop_front();
+            lk.unlock();
+
+            // Get topic and type from Json message and remove it.
+            std::string topic;
+            bool ret = com_private::getTopic(*pJsonDoc, topic);
+            assert(ret);
+            (void)ret;            
+
+
+            // Debug
+            rapidjson::StringBuffer sb;
+            rapidjson::Writer<rapidjson::StringBuffer> writer(sb);
+            pJsonDoc->Accept(writer);
+            std::cout << "message: " << sb.GetString() << std::endl;
+            std::cout << "topic: " << topic << std::endl;
+
+
+            ret = com_private::removeTopic(*pJsonDoc);
+            assert(ret);
+            (void)ret;
+
+            // Wait for topic (Rosbridge needs some time to process a advertise() request).
+            this->_rbc.waitForTopic(topic, [this]{
+                return this->_stopped->load();
+            });
+
+            // Publish Json message.
+            if ( !this->_stopped->load() )
+                this->_rbc.publish(topic, *pJsonDoc);
+        } // while loop
+#ifdef ROS_BRIDGE_DEBUG
+        std::cout << "TopicPublisher: publisher thread terminated." << std::endl;
+#endif
+    });
+}
+
+void ros_bridge::com_private::TopicPublisher::reset()
+{
+    if ( _stopped->load() )  // stop called while thread not running.
+        return;
+    std::unique_lock<std::mutex> lk(_mutex);
+    _stopped->store(true);
+    _cv.notify_one(); // Wake publisher thread.
+    lk.unlock();
+
+    if ( !_pThread )
+        return;
+    _pThread->join();
+
+    lk.lock();    
+    // Tidy up.
+    for (auto& it :  this->_topicMap){
+        this->_rbc.unadvertise(it.first);
+        this->_rbc.removeClient(it.second);
+    }
+    this->_topicMap.clear();
+    _queue.clear();
+}
+
+void ros_bridge::com_private::TopicPublisher::publish(
+        ros_bridge::com_private::JsonDocUPtr docPtr,
+        const char *topic)
+{
+    // Check if topic was advertised.
+    std::string t(topic);
+    std::unique_lock<std::mutex> lk(this->_mutex);
+    auto it =  this->_topicMap.find(t);
+    if ( it ==  this->_topicMap.end()) { // Not yet advertised?
+        lk.unlock();
+#ifdef ROS_BRIDGE_DEBUG
+        std::cerr << "TopicPublisher: topic "
+                  << t << " not yet advertised" << std::endl;
+#endif
+        return;
+    }
+    lk.unlock();
+
+    // Add topic information to json doc.
+    auto &allocator = docPtr->GetAllocator();
+    rapidjson::Value key("topic", allocator);
+    rapidjson::Value value(topic, allocator);
+    docPtr->AddMember(key, value, allocator);
+
+    lk.lock();
+    _queue.push_back(std::move(docPtr));
+    lk.unlock();
+    _cv.notify_one();  // Wake publisher thread.
+}
+
+bool ros_bridge::com_private::TopicPublisher::advertise(const char *topic, const char *type)
+{
+    std::unique_lock<std::mutex> lk(this->_mutex);
+    std::string t(topic);
+    auto it =  this->_topicMap.find(t);
+    if ( it ==  this->_topicMap.end()) { // Need to advertise topic?
+        std::string clientName =
+                std::string(ros_bridge::com_private::_topicAdvertiserKey)
+                + t;
+        this->_topicMap.insert(std::make_pair(t, clientName));
+        lk.unlock();
+        this->_rbc.addClient(clientName);
+        this->_rbc.advertise(clientName, t, type);
+        return true;
+    } else {
+        lk.unlock();
+#if ROS_BRIDGE_DEBUG
+        std::cerr << "TopicPublisher: topic " << topic << " already advertised" << std::endl;
+#endif
+        return false;
+    }
+}
+

src/comm/ros_bridge/include/TopicPublisher.h → src/comm/ros_bridge/include/topic_publisher.h

 #pragma once
 
-#include "ros_bridge/include/TypeFactory.h"
-#include "ros_bridge/include/CasePacker.h"
-#include "ros_bridge/include/ComPrivateInclude.h"
+#include "ros_bridge/include/com_private.h"
 #include "ros_bridge/include/RosBridgeClient.h"
 
 #include <thread>
-#include <deque>
 #include <atomic>
 #include <mutex>
-#include <set>
 #include <condition_variable>
 
-namespace ROSBridge {
-namespace ComPrivate {
+namespace ros_bridge {
+namespace com_private {
 
 struct ThreadData;
 
@@ -24,8 +20,7 @@ class TopicPublisher
 public:
 
     TopicPublisher() = delete;
-    TopicPublisher(CasePacker &casePacker,
-                   RosbridgeWsClient &rbc);
+    TopicPublisher(RosbridgeWsClient &rbc);
 
     ~TopicPublisher();
 
@@ -35,25 +30,15 @@ public:
     //! @brief Resets the publisher.
     void reset();
 
-    void publish(JsonDocUPtr docPtr){
-        {
-        std::lock_guard<std::mutex> lock(_mutex);
-        _queue.push_back(std::move(docPtr));
-        }
-        _cv.notify_one();  // Wake publisher thread.
-    }
-
-    template<class T>
-    void publish(const T &msg, const std::string &topic){
-        JsonDocUPtr docPtr(_casePacker.pack(msg, topic));
-        publish(std::move(docPtr));
-    }
+    void publish(JsonDocUPtr docPtr, const char *topic);
+    bool advertise(const char *topic, const char *type);
 
 private:
+    using TopicMap = std::unordered_map<std::string, std::string>;
     JsonQueue           _queue;
+    TopicMap            _topicMap;
     std::mutex          _mutex;
     std::shared_ptr<std::atomic_bool>   _stopped;
-    CasePacker         &_casePacker;
     RosbridgeWsClient  &_rbc;
     CondVar             _cv;
     ThreadPtr           _pThread;

src/comm/ros_bridge/include/TopicSubscriber.cpp → src/comm/ros_bridge/include/topic_subscriber.cpp

-#include "TopicSubscriber.h"
+#include "topic_subscriber.h"
 
 #include <thread>
 
-ROSBridge::ComPrivate::TopicSubscriber::TopicSubscriber(CasePacker &casePacker,
-        RosbridgeWsClient &rbc) :
-    _casePacker(casePacker)
-  , _rbc(rbc)
+ros_bridge::com_private::TopicSubscriber::TopicSubscriber(RosbridgeWsClient &rbc) :
+    _rbc(rbc)
   , _stopped(std::make_shared<std::atomic_bool>(true))
 {
 
 }
 
-ROSBridge::ComPrivate::TopicSubscriber::~TopicSubscriber()
+ros_bridge::com_private::TopicSubscriber::~TopicSubscriber()
 {
     this->reset();
 }
 
-void ROSBridge::ComPrivate::TopicSubscriber::start()
+void ros_bridge::com_private::TopicSubscriber::start()
 {
     _stopped->store(false);
 }
 
-void ROSBridge::ComPrivate::TopicSubscriber::reset()
+void ros_bridge::com_private::TopicSubscriber::reset()
 {
     if ( !_stopped->load() ){
-        std::cout << "TopicSubscriber: _stopped->store(true) " << std::endl;
         _stopped->store(true);
         {
             for (auto &item : _topicMap){
-                std::cout << "TopicSubscriber: unsubscribe " << item.second << std::endl;
                 _rbc.unsubscribe(item.second);
-                std::cout << "TopicSubscriber: removeClient " << item.first << std::endl;
                 _rbc.removeClient(item.first);
             }
         }
         _topicMap.clear();
-        std::cout << "TopicSubscriber: _topicMap cleared " << std::endl;
     }
 }
 
-void ROSBridge::ComPrivate::TopicSubscriber::subscribe(
+void ros_bridge::com_private::TopicSubscriber::subscribe(
         const char *topic,
         const std::function<void(JsonDocUPtr)> &callback)
 {
     if ( _stopped->load() )
         return;
 
-    std::string clientName = ROSBridge::ComPrivate::_topicSubscriberKey
+    std::string clientName = ros_bridge::com_private::_topicSubscriberKey
                            + std::string(topic);
     auto it = _topicMap.find(clientName);
     if ( it != _topicMap.end() ){ // Topic already subscribed?
@@ -55,7 +49,6 @@ void ROSBridge::ComPrivate::TopicSubscriber::subscribe(
     _topicMap.insert(std::make_pair(clientName, std::string(topic)));
 
     // Wrap callback.
-    using namespace std::placeholders;
     auto callbackWrapper = [callback](
             std::shared_ptr<WsClient::Connection>,
             std::shared_ptr<WsClient::InMessage> in_message)

src/comm/ros_bridge/include/TopicSubscriber.h → src/comm/ros_bridge/include/topic_subscriber.h

 #pragma once
 
-#include "ros_bridge/include/ComPrivateInclude.h"
+#include "ros_bridge/include/com_private.h"
 #include "ros_bridge/include/RosBridgeClient.h"
-#include "ros_bridge/include/TypeFactory.h"
-#include "ros_bridge/include/CasePacker.h"
 
 #include <unordered_map>
 
-namespace ROSBridge {
-namespace ComPrivate {
+namespace ros_bridge {
+namespace com_private {
 
 
 typedef std::function<void(JsonDocUPtr)> CallbackType;
@@ -19,7 +17,7 @@ class TopicSubscriber
 public:
 
     TopicSubscriber() = delete;
-    TopicSubscriber(CasePacker &casePacker, RosbridgeWsClient &rbc);
+    TopicSubscriber(RosbridgeWsClient &rbc);
     ~TopicSubscriber();
 
     //! @brief Starts the subscriber.
@@ -34,10 +32,6 @@ public:
     void subscribe(const char* topic, const CallbackType &callback);
 
 private:
-
-
-
-    CasePacker         &_casePacker;
     RosbridgeWsClient   &_rbc;
     TopicMap            _topicMap;
     std::shared_ptr<std::atomic_bool>   _stopped;

src/comm/ros_bridge/src/CasePacker.cpp

-#include "ros_bridge/include/CasePacker.h"
-
-const char* ROSBridge::CasePacker::topicKey         = "topic";
-const char* ROSBridge::CasePacker::messageTypeKey   = "messageType";
-
-ROSBridge::CasePacker::CasePacker(TypeFactory *typeFactory, JsonFactory *jsonFactory) :
-    _typeFactory(typeFactory)
-  , _jsonFactory(jsonFactory)
-{
-
-}
-
-bool ROSBridge::CasePacker::getTag(const JsonDocUPtr &pDoc, Tag &tag) const
-{
-    if( !getTopic(pDoc, tag.topic()) )
-        return false;
-    if( !getMessageType(pDoc, tag.messageType()) )
-        return false;
-    return true;
-}
-
-void ROSBridge::CasePacker::addTag(JsonDocUPtr &pDoc, const std::string &topic, const std::string &messageType) const
-{
-    using namespace ROSBridge;
-    using namespace rapidjson;
-
-    {
-    // add topic
-    rapidjson::Value key(CasePacker::topicKey, pDoc->GetAllocator());
-    rapidjson::Value value(topic.c_str(), pDoc->GetAllocator());
-    pDoc->AddMember(key, value, pDoc->GetAllocator());
-    }
-
-    // add messageType
-    rapidjson::Value key(CasePacker::messageTypeKey, pDoc->GetAllocator());
-    rapidjson::Value value(messageType.c_str(), pDoc->GetAllocator());
-    pDoc->AddMember(key, value, pDoc->GetAllocator());
-}
-
-void ROSBridge::CasePacker::addTag(JsonDocUPtr &doc, const ROSBridge::CasePacker::Tag &tag) const
-{
-    addTag(doc, tag.topic(), tag.messageType());
-}
-
-void ROSBridge::CasePacker::removeTag(JsonDocUPtr &pDoc) const
-{
-    using namespace ROSBridge;
-    using namespace rapidjson;
-    if ( pDoc->HasMember(CasePacker::topicKey) )
-        pDoc->RemoveMember(CasePacker::topicKey);
-    if ( pDoc->HasMember(CasePacker::messageTypeKey) )
-        pDoc->RemoveMember(CasePacker::messageTypeKey);
-}
-
-bool ROSBridge::CasePacker::getTopic(const JsonDocUPtr &pDoc, std::string &topic) const
-{
-    if (!pDoc->HasMember(CasePacker::topicKey) || !(*pDoc)[CasePacker::topicKey].IsString())
-        return false;
-    topic = (*pDoc)[CasePacker::topicKey].GetString();
-    return true;
-}
-
-bool ROSBridge::CasePacker::getMessageType(const JsonDocUPtr&pDoc, std::string &messageType) const
-{
-    if (!pDoc->HasMember(CasePacker::messageTypeKey) || !(*pDoc)[CasePacker::messageTypeKey].IsString())
-        return false;
-    messageType = (*pDoc)[CasePacker::messageTypeKey].GetString();
-    return true;
-}

src/comm/ros_bridge/src/ROSBridge.cpp → src/comm/ros_bridge/src/ros_bridge.cpp

-#include "ros_bridge/include/ROSBridge.h"
+#include "ros_bridge/include/ros_bridge.h"
 
-ROSBridge::ROSBridge::ROSBridge(const char *connectionString) :
+
+ros_bridge::ROSBridge::ROSBridge(const char *connectionString) :
     _stopped(std::make_shared<std::atomic_bool>(true))
-  ,  _casePacker(&_typeFactory, &_jsonFactory)
   , _rbc(connectionString, false /*run*/)
-  , _topicPublisher(_casePacker, _rbc)
-  , _topicSubscriber(_casePacker, _rbc)
-  , _server(_casePacker, _rbc)
+  , _topicPublisher(_rbc)
+  , _topicSubscriber(_rbc)
+  , _server(_rbc)
 {
 }
 
-ROSBridge::ROSBridge::ROSBridge() :
-    ROSBridge::ROSBridge::ROSBridge("localhost:9090")
+ros_bridge::ROSBridge::ROSBridge() :
+    ros_bridge::ROSBridge::ROSBridge("localhost:9090")
 {
 }
 
-void ROSBridge::ROSBridge::publish(ROSBridge::ROSBridge::JsonDocUPtr doc)
+void ros_bridge::ROSBridge::publish(ros_bridge::ROSBridge::JsonDocUPtr doc, const char* topic)
 {
-    _topicPublisher.publish(std::move(doc));
+    _topicPublisher.publish(std::move(doc), topic);
 }
 
-void ROSBridge::ROSBridge::subscribe(const char *topic, const std::function<void(JsonDocUPtr)> &callBack)
+void ros_bridge::ROSBridge::subscribe(const char *topic, const std::function<void(JsonDocUPtr)> &callBack)
 {
     _topicSubscriber.subscribe(topic, callBack);
 }
 
-void ROSBridge::ROSBridge::advertiseService(const std::string &service,
-                                            const std::string &type,
+void ros_bridge::ROSBridge::advertiseService(const char *service,
+                                             const char *type,
                                              const std::function<JsonDocUPtr(JsonDocUPtr)> &callback)
 {
     _server.advertiseService(service, type, callback);
 }
 
-const ROSBridge::CasePacker *ROSBridge::ROSBridge::casePacker() const
+void ros_bridge::ROSBridge::advertiseTopic(const char *topic, const char *type)
 {
-    return &_casePacker;
+    _topicPublisher.advertise(topic, type);
 }
 
-void ROSBridge::ROSBridge::start()
+void ros_bridge::ROSBridge::start()
 {
+    if ( !_stopped->load() )
+        return;
     _stopped->store(false);
     _rbc.run();
     _topicPublisher.start();
@@ -46,9 +48,11 @@ void ROSBridge::ROSBridge::start()
     _server.start();
 }
 
-void ROSBridge::ROSBridge::reset()
+void ros_bridge::ROSBridge::reset()
 {
     auto start = std::chrono::high_resolution_clock::now();
+    if ( _stopped->load() )
+        return;
     _stopped->store(true);
     _topicPublisher.reset();
     _topicSubscriber.reset();
@@ -56,21 +60,22 @@ void ROSBridge::ROSBridge::reset()
     _rbc.reset();
     auto end = std::chrono::high_resolution_clock::now();
     auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
-    std::cout << "ROSBridge reset duration: " << delta << " ms" << std::endl;
+    std::cout << "ros_bridge reset duration: " << delta << " ms" << std::endl;
 }
 
-bool ROSBridge::ROSBridge::connected()
+bool ros_bridge::ROSBridge::connected()
 {
+    start();
     return _rbc.connected();
 }
 
-bool ROSBridge::ROSBridge::isRunning()
+bool ros_bridge::ROSBridge::isRunning()
 {
     return !_stopped->load();
 }
 
 
-bool ROSBridge::isValidConnectionString(const char *connectionString)
+bool ros_bridge::isValidConnectionString(const char *connectionString)
 {
     return is_valid_port_path(connectionString);
 }

src/comm/utilities.h

@@ -73,15 +73,4 @@ private:
     bool            _isInitialized;
 };
 
-template <typename T>
-struct Type2Type{
-    typedef T OriginalType;
-};
-
-
-template <int k>
-struct Int2Type{
-    enum {value = k};
-};
-
 #endif // UTILITIES_H