diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index f8045a1b84f335eeccf338d7fadb5f741f56e89c..d89ce3ef097d27670d7fe241bc039fd3df8d0032 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -516,6 +516,7 @@ SOURCES += \
     src/Wima/WimaBridge.cc \
     src/comm/ros_bridge/include/RosBridgeClient.cpp \
     src/comm/ros_bridge/include/com_private.cpp \
+    src/comm/ros_bridge/include/messages/std_msgs/header.cpp \
     src/comm/ros_bridge/include/server.cpp \
     src/comm/ros_bridge/include/topic_publisher.cpp \
     src/comm/ros_bridge/include/topic_subscriber.cpp \
diff --git a/src/Wima/Geometry/GeoPoint3D.cpp b/src/Wima/Geometry/GeoPoint3D.cpp
index 5f0165866e88da5c2c3d188ef1dc710305ef1d9f..2d5bd3d59366fa4e1c764f943e148dda65197651 100644
--- a/src/Wima/Geometry/GeoPoint3D.cpp
+++ b/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());
diff --git a/src/Wima/Geometry/GeoPoint3D.h b/src/Wima/Geometry/GeoPoint3D.h
index 5f61a9f831ec41f08f9262c6b920acd56fefa73b..63b221d117e52c061d27e3f48bcea27e3cc7f3f4 100644
--- a/src/Wima/Geometry/GeoPoint3D.h
+++ b/src/Wima/Geometry/GeoPoint3D.h
@@ -3,14 +3,14 @@
 #include <QGeoCoordinate>
 #include <QObject>
 
+#include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
 
-typedef ros_bridge::GenericMessages::GeographicMsgs::GeoPoint ROSGeoPoint;
-namespace MsgGroups = ros_bridge::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,
@@ -24,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);
 
diff --git a/src/Wima/Geometry/Polygon2D.h b/src/Wima/Geometry/Polygon2D.h
index 7c4595e9b310ce5e207b7e7882d4e0619aed7d62..d9390a5ad4cc795572dfb558eeb2f025e22ceddd 100644
--- a/src/Wima/Geometry/Polygon2D.h
+++ b/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 = ros_bridge::MessageGroups;
-namespace PolyStampedNS = ros_bridge::JsonMethodes::GeometryMsgs::PolygonStamped;
-
-typedef ros_bridge::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 ros_bridge::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;
 
 };
 
diff --git a/src/Wima/Geometry/PolygonArray.h b/src/Wima/Geometry/PolygonArray.h
index bd933604b409ad4e48db7b9a8b03917bee9b2b93..3d92547e2a4541f1db50bcaa65cbd6fce7348830 100644
--- a/src/Wima/Geometry/PolygonArray.h
+++ b/src/Wima/Geometry/PolygonArray.h
@@ -1,14 +1,11 @@
 #pragma once
 
-#include <QObject>
+#include <QString>
 
-#include "ros_bridge/include/MessageBaseClass.h"
-
-typedef ros_bridge::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";}
diff --git a/src/Wima/Snake/SnakeTilesLocal.h b/src/Wima/Snake/SnakeTilesLocal.h
index 1a81aadc7ef673a8761c35c229e18f592dd13a29..ceebbacd9f0ef7fd003ee3831ba13cdd9f2d165c 100644
--- a/src/Wima/Snake/SnakeTilesLocal.h
+++ b/src/Wima/Snake/SnakeTilesLocal.h
@@ -1,6 +1,5 @@
 #pragma once
 
-#include "ros_bridge/include/MessageGroups.h"
 #include "Wima/Geometry/Polygon2D.h"
 #include "Wima/Geometry/WimaPolygonArray.h"
 
diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc
index fb4040ef987262451e5d075f918afccaea833007..b4990055dd9328bfcae20d3a7e435104b8a9b603 100644
--- a/src/Wima/WimaController.cc
+++ b/src/Wima/WimaController.cc
@@ -10,8 +10,6 @@
 #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"
 
diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h
index 8535a4fe0b524600c89447159fad7ae5b9d1434d..24f53a796d7a072e0725c139d57408c4b82c300f 100644
--- a/src/Wima/WimaController.h
+++ b/src/Wima/WimaController.h
@@ -40,6 +40,8 @@
 #include "WaypointManager/DefaultManager.h"
 #include "WaypointManager/RTLManager.h"
 
+#include "utilities.h"
+
 #include <map>
 
 
@@ -343,7 +345,6 @@ private slots:
 
 private:
     using StatusMap = std::map<int, QString>;
-    using CasePacker = ros_bridge::CasePacker;
 
     // Controllers.
     PlanMasterController   *_masterController;
diff --git a/src/comm/ros_bridge/include/com_private.h b/src/comm/ros_bridge/include/com_private.h
index ada0ab662eed77d67b008cb1ee63145340371975..c5dcffead3fe1562d2c7518de2e00768752b2fc8 100644
--- a/src/comm/ros_bridge/include/com_private.h
+++ b/src/comm/ros_bridge/include/com_private.h
@@ -1,6 +1,5 @@
 #pragma once
 
-#include "ros_bridge/include/MessageTag.h"
 #include "ros_bridge/rapidjson/include/rapidjson/document.h"
 
 #include <memory>
@@ -10,7 +9,6 @@
 namespace ros_bridge {
 namespace com_private {
 
-typedef MessageTag Tag;
 typedef rapidjson::Document JsonDoc;
 typedef std::unique_ptr<JsonDoc> JsonDocUPtr;
 typedef std::deque<JsonDocUPtr> JsonQueue;
diff --git a/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h b/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h
index 799687c4ef58dbde6f504ccc02c25b01d9f91550..31bdbe43ea970fb80f06314d2e2b2b0e1e85e8a7 100644
--- a/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h
+++ b/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h
@@ -111,7 +111,9 @@ bool toJson(const T&p, rapidjson::Value &value, rapidjson::Document::AllocatorTy
     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);
-    value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), allocator);
+    rapidjson::Value jType;
+    jType.SetString(messageType().c_str(), allocator);
+    value.AddMember("type", jType, allocator);
     return true;
 }
 
diff --git a/src/comm/ros_bridge/include/messages/geometry_msgs/point32.h b/src/comm/ros_bridge/include/messages/geometry_msgs/point32.h
index 6788a43d9992382d1e14479030bb5df740e1e71d..2d0bb2c1f4f1c125a6d435b37323c32d38c6e01c 100644
--- a/src/comm/ros_bridge/include/messages/geometry_msgs/point32.h
+++ b/src/comm/ros_bridge/include/messages/geometry_msgs/point32.h
@@ -105,7 +105,9 @@ bool toJson(const T&p, rapidjson::Value &value, rapidjson::Document::AllocatorTy
     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);
-    value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), allocator);
+    rapidjson::Value jType;
+    jType.SetString(messageType().c_str(), allocator);
+    value.AddMember("type", jType, allocator);
     return true;
 }
 
diff --git a/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h
index 75b2f97df38bc109b6658db325f3aa455e8ecfe7..f9006a5b9e9f839aecefaa34d6540cda9330d058 100644
--- a/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h
+++ b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h
@@ -22,7 +22,7 @@ std::string messageType(){
 template <class PointTypeCVR,
           template <class, class...> class ContainerType = std::vector>
 class GenericPolygon {
-    typedef typename std::remove_cv_ref_t<PointTypeCVR> PointType;
+    typedef typename std::remove_cv_t<typename std::remove_reference_t<PointTypeCVR>> PointType;
 public:
     GenericPolygon() {}
     GenericPolygon(const GenericPolygon &poly) : _points(poly.points()){}
@@ -59,8 +59,9 @@ bool toJson(const PolygonType &poly, rapidjson::Value &value, rapidjson::Documen
     }
 
     value.AddMember("points", points, allocator);
-    value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), allocator);
-
+    rapidjson::Value jType;
+    jType.SetString(messageType().c_str(), allocator);
+    value.AddMember("type", jType, allocator);
     return true;
 }
 
diff --git a/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h
index da145c56001fcb6e05e21252abd546fd9710d37c..f7616fdec6d5cdfdf126db9295484c6a2e68f55a 100644
--- a/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h
+++ b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h
@@ -57,12 +57,12 @@ template <class PolygonStampedType>
 bool setHeader(const rapidjson::Value &doc,
                PolygonStampedType &polyStamped,
                traits::Int2Type<1>) { // polyStamped.header() exists
-    using namespace std_msgs::header;
+    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);
+    bool ret = header::fromJson(doc, header);
     polyStamped.header() = header;
     return ret;
 }
@@ -82,8 +82,8 @@ bool _toJson(const PolygonType &poly,
              rapidjson::Value &value,
              rapidjson::Document::AllocatorType &allocator)
 {
-    using namespace std_msgs::header;
-    using namespace geometry_msgs::polygon;
+    using namespace std_msgs;
+    using namespace geometry_msgs;
 
     rapidjson::Document header(rapidjson::kObjectType);
     if (!header::toJson(h, header, allocator)){
@@ -97,7 +97,9 @@ bool _toJson(const PolygonType &poly,
     }
     value.AddMember("header", header, allocator);
     value.AddMember("polygon", polygon, allocator);
-    value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), allocator);
+    rapidjson::Value jType;
+    jType.SetString(messageType().c_str(), allocator);
+    value.AddMember("type", jType, allocator);
 
     return true;
 }
diff --git a/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h b/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h
index 7da61863d94a4a1c848f3bc0d264227a9c58eb4f..9407b0341f4c5589fae55a52a174cfde72d76e74 100644
--- a/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h
+++ b/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h
@@ -152,8 +152,8 @@ namespace detail {
                  rapidjson::Value &value,
                  rapidjson::Document::AllocatorType &allocator)
     {
-        using namespace std_msgs::header;
-        using namespace geometry_msgs::polygon_stamped;
+        using namespace std_msgs;
+        using namespace geometry_msgs;
 
         rapidjson::Document jHeader(rapidjson::kObjectType);
         if (!header::toJson(h, jHeader, allocator)){
@@ -185,7 +185,9 @@ namespace detail {
         detail::likelihoodToJson(p, JLikelihood, allocator, traits::Int2Type<HasLikelihood::value>());
         value.AddMember("likelihood", JLikelihood, allocator);
 
-        value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), allocator);
+        rapidjson::Value jType;
+        jType.SetString(messageType().c_str(), allocator);
+        value.AddMember("type", jType, allocator);
 
         return true;
     }
diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h b/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h
index 50434997d1611d21ce705191b2b65ee3a5fe5b77..ce783f4d6a6e2c2710a5a987da51676e48db078d 100644
--- a/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h
+++ b/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h
@@ -34,7 +34,9 @@ template <class HeartbeatType>
 bool toJson(const HeartbeatType &heartbeat, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
 {
     value.AddMember("status", std::int32_t(heartbeat.status()), allocator);
-    value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), allocator);
+    rapidjson::Value jType;
+    jType.SetString(messageType().c_str(), allocator);
+    value.AddMember("type", jType, allocator);
     return true;
 }
 
diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/progress.h b/src/comm/ros_bridge/include/messages/nemo_msgs/progress.h
index 589cf8f179674e05d3c6ad990670aac216feebfc..7825acf906e5f369d7b652e3fa5d5e58e59e4cdf 100644
--- a/src/comm/ros_bridge/include/messages/nemo_msgs/progress.h
+++ b/src/comm/ros_bridge/include/messages/nemo_msgs/progress.h
@@ -43,7 +43,9 @@ bool toJson(const ProgressType &p, rapidjson::Value &value, rapidjson::Document:
         jProgress.PushBack(rapidjson::Value().SetInt(std::int32_t(p.progress()[i])), allocator);
     }
     value.AddMember("progress", jProgress, allocator);
-    value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), allocator);
+    rapidjson::Value jType;
+    jType.SetString(messageType().c_str(), allocator);
+    value.AddMember("type", jType, allocator);
     return true;
 }
 
diff --git a/src/comm/ros_bridge/include/messages/std_msgs/header.cpp b/src/comm/ros_bridge/include/messages/std_msgs/header.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5c108a077b4f1149e39e95d4ee66245eb0860b8e
--- /dev/null
+++ b/src/comm/ros_bridge/include/messages/std_msgs/header.cpp
@@ -0,0 +1,65 @@
+#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;
+}
diff --git a/src/comm/ros_bridge/include/messages/std_msgs/header.h b/src/comm/ros_bridge/include/messages/std_msgs/header.h
index 55ac9de093b16f76e18ea1ecaac18f26af22af85..066f9d4c8a66c3e88705ed9c2fd42ca12c3a4026 100644
--- a/src/comm/ros_bridge/include/messages/std_msgs/header.h
+++ b/src/comm/ros_bridge/include/messages/std_msgs/header.h
@@ -22,29 +22,22 @@ std::string messageType(){
 class Header{
 public:
     using Time = std_msgs::time::Time;
-    Header() : _seq(_defaultSeq++), _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;}
+    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 = 0;
+    static uint32_t _defaultSeq;
     uint32_t    _seq;
     Time        _stamp;
     std::string _frameId;
@@ -53,11 +46,14 @@ private:
 template <class HeaderType>
 bool toJson(const HeaderType &header,
             rapidjson::Value &value,
-            rapidjson::Document::AllocatorType &allocator)     {
+            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)){
+    if (!time::toJson(header.stamp(), stamp, allocator)){
         assert(false);
         return false;
     }
@@ -68,14 +64,19 @@ bool toJson(const HeaderType &header,
                                                header.frameId().length(),
                                                allocator),
                   allocator);
-    value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), allocator);
+    rapidjson::Value jType;
+    jType.SetString(messageType().c_str(), allocator);
+    value.AddMember("type", jType, allocator);
 
     return true;
 }
 
 template <class HeaderType>
 bool fromJson(const rapidjson::Value &value,
-              HeaderType &header)     {
+              HeaderType &header)
+{
+    using namespace messages::std_msgs;
+
     if (!value.HasMember("seq")|| !value["seq"].IsUint()){
         assert(false);
         return false;
@@ -90,7 +91,7 @@ bool fromJson(const rapidjson::Value &value,
     }
     header.setSeq(value["seq"].GetUint());
     decltype(header.stamp()) time;
-    if (!Time::fromJson(value["stamp"], time)){
+    if (!time::fromJson(value["stamp"], time)){
         assert(false);
         return false;
     }
diff --git a/src/comm/ros_bridge/include/messages/std_msgs/time.h b/src/comm/ros_bridge/include/messages/std_msgs/time.h
index b8d70643ffb93fdb6182f2c2763b5b09b0a77306..d88e51c8c133f8b8c87917a7cd797249d44f1893 100644
--- a/src/comm/ros_bridge/include/messages/std_msgs/time.h
+++ b/src/comm/ros_bridge/include/messages/std_msgs/time.h
@@ -39,7 +39,9 @@ bool toJson(const TimeType &time,
 {
     value.AddMember("secs", rapidjson::Value().SetUint(uint32_t(time.secs())), allocator);
     value.AddMember("nsecs", rapidjson::Value().SetUint(uint32_t(time.nSecs())), allocator);
-    value.AddMember("type", rapidjson::Value().SetString(messageType().c_str()), allocator);
+    rapidjson::Value jType;
+    jType.SetString(messageType().c_str(), allocator);
+    value.AddMember("type", jType, allocator);
 
     return true;
 }
diff --git a/src/comm/ros_bridge/include/ros_bridge.h b/src/comm/ros_bridge/include/ros_bridge.h
index 0bb9e12c0dc1b88c0109a7cd36ce1f86470ce8cf..46d9aa59caf1e593a61bc76f1605837e6b85fea0 100644
--- a/src/comm/ros_bridge/include/ros_bridge.h
+++ b/src/comm/ros_bridge/include/ros_bridge.h
@@ -1,9 +1,6 @@
 #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/topic_publisher.h"
 #include "ros_bridge/include/topic_subscriber.h"
 #include "ros_bridge/include/server.h"
diff --git a/src/comm/ros_bridge/include/server.cpp b/src/comm/ros_bridge/include/server.cpp
index c1a335a71d81efed35f3ebfb03c2a9020ec48aa3..eca731e71acb3366cb1a9e1cd786317239d32a15 100644
--- a/src/comm/ros_bridge/include/server.cpp
+++ b/src/comm/ros_bridge/include/server.cpp
@@ -2,9 +2,8 @@
 
 #include "rapidjson/include/rapidjson/ostreamwrapper.h"
 
-ros_bridge::com_private::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))
 {
 
@@ -22,9 +21,8 @@ void ros_bridge::com_private::Server::advertiseService(const std::string &servic
     _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
diff --git a/src/comm/ros_bridge/include/server.h b/src/comm/ros_bridge/include/server.h
index 8a8145cc6837139a7f7a1aa89eec6cc862529bbe..f1500b9ed7de552bcc3e7a686189c70afe5d026e 100644
--- a/src/comm/ros_bridge/include/server.h
+++ b/src/comm/ros_bridge/include/server.h
@@ -2,8 +2,6 @@
 
 #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>
 
@@ -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;
 };
diff --git a/src/comm/ros_bridge/include/topic_publisher.cpp b/src/comm/ros_bridge/include/topic_publisher.cpp
index a2d4aa62a1bdd2deee8feaca9b0dafce5faf4416..6ae5f20aff857b849d4ba7e105c5e30d7d15d816 100644
--- a/src/comm/ros_bridge/include/topic_publisher.cpp
+++ b/src/comm/ros_bridge/include/topic_publisher.cpp
@@ -2,10 +2,8 @@
 
 #include <unordered_map>
 
-ros_bridge::com_private::TopicPublisher::TopicPublisher(CasePacker &casePacker,
-                                                      RosbridgeWsClient &rbc) :
+ros_bridge::com_private::TopicPublisher::TopicPublisher(RosbridgeWsClient &rbc) :
       _stopped(std::make_shared<std::atomic_bool>(true))
-    , _casePacker(casePacker)
     , _rbc(rbc)
 {
 
@@ -95,8 +93,11 @@ void ros_bridge::com_private::TopicPublisher::reset()
 
 void ros_bridge::com_private::TopicPublisher::publish(
         ros_bridge::com_private::JsonDocUPtr docPtr,
-        const char *topic){
-    docPtr->AddMember("topic", topic, doc->GetAllocator());
+        const char *topic)
+{
+    rapidjson::Value jTopic;
+    jTopic.SetString(topic, docPtr->GetAllocator());
+    docPtr->AddMember("topic", jTopic, docPtr->GetAllocator());
     std::unique_lock<std::mutex> lock(_mutex);
     _queue.push_back(std::move(docPtr));
     lock.unlock();
diff --git a/src/comm/ros_bridge/include/topic_publisher.h b/src/comm/ros_bridge/include/topic_publisher.h
index eec9c8662fb2cec07d44981c6084b18f0b98d787..8c3a21259afeb11c55680cea42013b09382bca83 100644
--- a/src/comm/ros_bridge/include/topic_publisher.h
+++ b/src/comm/ros_bridge/include/topic_publisher.h
@@ -20,8 +20,7 @@ class TopicPublisher
 public:
 
     TopicPublisher() = delete;
-    TopicPublisher(CasePacker &casePacker,
-                   RosbridgeWsClient &rbc);
+    TopicPublisher(RosbridgeWsClient &rbc);
 
     ~TopicPublisher();