diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index d89ce3ef097d27670d7fe241bc039fd3df8d0032..9697d482014966f0b705f218e6b247eed0b5f09e 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -32,7 +32,7 @@ DebugBuild {
 }
 else {
     DESTDIR  = $${OUT_PWD}/release
-    #DEFINES += DEBUG
+    DEFINES += DEBUG
     DEFINES += NDEBUG
 }
 
@@ -494,7 +494,7 @@ HEADERS += \
     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/time.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
@@ -516,7 +516,15 @@ 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/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 \
diff --git a/src/Wima/Snake/QNemoProgress.h b/src/Wima/Snake/QNemoProgress.h
index 0d838984b3f29af1423fb7e0f187cf73f9f0bad7..e57ec2fdf108993dff80f7d5544b6d10a77c39c2 100644
--- a/src/Wima/Snake/QNemoProgress.h
+++ b/src/Wima/Snake/QNemoProgress.h
@@ -4,5 +4,5 @@
 
 #include "ros_bridge/include/messages/nemo_msgs/progress.h"
 
-namespace nemo_msgs = ros_bridge::messages::nemo_msgs;
-typedef nemo_msgs::progress::GenericProgress<int, QVector> QNemoProgress;
+namespace nemo = ros_bridge::messages::nemo_msgs;
+typedef nemo::progress::GenericProgress<int, QVector> QNemoProgress;
diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc
index b4990055dd9328bfcae20d3a7e435104b8a9b603..5ddcf2669719bee8949388284541b73e14a03318 100644
--- a/src/Wima/WimaController.cc
+++ b/src/Wima/WimaController.cc
@@ -554,8 +554,17 @@ bool WimaController::setWimaPlanData(const WimaPlanData &planData)
             && _pRosBridge->isRunning()
             && _pRosBridge->connected() ){
         if ( _snakeTilesLocal.polygons().size() > 0 ){
-            _pRosBridge->publish(_snakeOrigin, "/snake/origin");
-            _pRosBridge->publish(_snakeTilesLocal, "/snake/tiles");
+            using namespace ros_bridge::messages;
+            // Publish snake origin.
+            JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
+            Q_ASSERT(geographic_msgs::geo_point::toJson(
+                         _snakeOrigin, *jOrigin, jOrigin->GetAllocator()));
+            _pRosBridge->publish(std::move(jOrigin), "/snake/origin");
+            // Publish snake tiles.
+            JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
+            Q_ASSERT(jsk_recognition_msgs::polygon_array::toJson(
+                         _snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator()));
+            _pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
         }
     }
 
@@ -944,12 +953,12 @@ void WimaController::_setupTopicService()
     using namespace ros_bridge::messages;
     if ( _snakeTilesLocal.polygons().size() > 0){
         // Publish snake origin.
-        JsonDocUPtr jOrigin(rapidjson::kObjectType);
+        JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
         Q_ASSERT(geographic_msgs::geo_point::toJson(
                      _snakeOrigin, *jOrigin, jOrigin->GetAllocator()));
         _pRosBridge->publish(std::move(jOrigin), "/snake/origin");
         // Publish snake tiles.
-        JsonDocUPtr jSnakeTiles(rapidjson::kObjectType);
+        JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
         Q_ASSERT(jsk_recognition_msgs::polygon_array::toJson(
                      _snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator()));
         _pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
@@ -966,12 +975,12 @@ void WimaController::_setupTopicService()
             // Publish origin and tiles again, if valid.
             if ( this->_snakeTilesLocal.polygons().size() > 0){
                 // Publish snake origin.
-                JsonDocUPtr jOrigin(rapidjson::kObjectType);
+                JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
                 Q_ASSERT(geographic_msgs::geo_point::toJson(
                              this->_snakeOrigin, *jOrigin, jOrigin->GetAllocator()));
                 this->_pRosBridge->publish(std::move(jOrigin), "/snake/origin");
                 // Publish snake tiles.
-                JsonDocUPtr jSnakeTiles(rapidjson::kObjectType);
+                JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
                 Q_ASSERT(jsk_recognition_msgs::polygon_array::toJson(
                              this->_snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator()));
                 this->_pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
@@ -983,7 +992,7 @@ void WimaController::_setupTopicService()
     _pRosBridge->subscribe("/nemo/heartbeat",
                            /* callback */ [this](JsonDocUPtr pDoc){
         auto &heartbeat_msg = this->_nemoHeartbeat;
-        if ( !nemo_msgs::heartbeat(*pDoc, heartbeat_msg) ) {
+        if ( !nemo_msgs::heartbeat::fromJson(*pDoc, heartbeat_msg) ) {
             if ( heartbeat_msg.status() == this->_fallbackStatus )
                 return;
             heartbeat_msg.setStatus(this->_fallbackStatus);
@@ -998,13 +1007,14 @@ void WimaController::_setupTopicService()
     _pRosBridge->advertiseService("/snake/get_origin", "snake_msgs/GetOrigin",
                                   [this](JsonDocUPtr) -> JsonDocUPtr
     {
-        JsonDocUPtr pDoc(std::make_unique(rapidjson::kObjectType));
+        using namespace ros_bridge::messages;
+        JsonDocUPtr pDoc(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
         rapidjson::Value jOrigin(rapidjson::kObjectType);
         if ( this->_snakeTilesLocal.polygons().size() > 0){
-            geometry_msg::geo_point::toJson(
+            geographic_msgs::geo_point::toJson(
                         this->_snakeOrigin, jOrigin, pDoc->GetAllocator());
         } else {
-            geometry_msg::geo_point::polygon_array::toJson(
+            geographic_msgs::geo_point::toJson(
                         ::GeoPoint3D(0,0,0), jOrigin, pDoc->GetAllocator());
         }
         pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator());
@@ -1014,7 +1024,7 @@ void WimaController::_setupTopicService()
     _pRosBridge->advertiseService("/snake/get_tiles", "snake_msgs/GetTiles",
                                   [this](JsonDocUPtr) -> JsonDocUPtr
     {
-        JsonDocUPtr pDoc(std::make_unique(rapidjson::kObjectType));
+        JsonDocUPtr pDoc(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
         rapidjson::Value jSnakeTiles(rapidjson::kObjectType);
         jsk_recognition_msgs::polygon_array::toJson(
                     this->_snakeTilesLocal, jSnakeTiles, pDoc->GetAllocator());
diff --git a/src/comm/ros_bridge/include/com_private.cpp b/src/comm/ros_bridge/include/com_private.cpp
index 001be47e946eb3f871a9ee0e2eeb26b646992a05..4d5365beb71adcd8c62d0a68e2cd4d1ab7f71237 100644
--- a/src/comm/ros_bridge/include/com_private.cpp
+++ b/src/comm/ros_bridge/include/com_private.cpp
@@ -1,5 +1,9 @@
 #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)
 {
@@ -15,7 +19,19 @@ std::size_t ros_bridge::com_private::getHash(const char *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();
+
+
+        rapidjson::StringBuffer sb1;
+        rapidjson::Writer<rapidjson::StringBuffer> writer1(sb1);
+        doc.Accept(writer1);
+        std::cout << "getTopic doc: " << sb1.GetString() << std::endl;
+
+        rapidjson::StringBuffer sb;
+        rapidjson::Writer<rapidjson::StringBuffer> writer(sb);
+        doc["topic"].Accept(writer);
+        topic = sb.GetString();
+
+        std::cout << "getTopic topic: " << sb.GetString() << std::endl;
         return true;
     } else
         return false;
@@ -33,7 +49,10 @@ bool ros_bridge::com_private::removeTopic(ros_bridge::com_private::JsonDoc &doc)
 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();
+        rapidjson::StringBuffer sb;
+        rapidjson::Writer<rapidjson::StringBuffer> writer(sb);
+        doc["type"].Accept(writer);
+        type = sb.GetString();
         return true;
     } else
         return false;
diff --git a/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.cpp b/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c6fc8a4cb40d09d7b56f0a57e1c8f87c77949de2
--- /dev/null
+++ b/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.cpp
@@ -0,0 +1,5 @@
+#include "geopoint.h"
+
+std::string ros_bridge::messages::geographic_msgs::geo_point::messageType(){
+    return "geometry_msgs/GeoPoint";
+}
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 31bdbe43ea970fb80f06314d2e2b2b0e1e85e8a7..a28cec323cd566c14d613a2e6593613b045404fd 100644
--- a/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h
+++ b/src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h
@@ -13,9 +13,7 @@ namespace geographic_msgs {
 //! @brief Namespace containing methodes for geometry_msgs/GeoPoint message generation.
 namespace geo_point {
 
-std::string messageType(){
-    return "geometry_msgs/GeoPoint";
-}
+std::string messageType();
 
 //! @brief C++ representation of geographic_msgs/GeoPoint.
 template<class FloatType = _Float64, class OStream = std::ostream>
diff --git a/src/comm/ros_bridge/include/messages/geometry_msgs/point32.cpp b/src/comm/ros_bridge/include/messages/geometry_msgs/point32.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2736636eda3d28c21bc0cb3fd84d8ca9c80ba69f
--- /dev/null
+++ b/src/comm/ros_bridge/include/messages/geometry_msgs/point32.cpp
@@ -0,0 +1,6 @@
+#include "point32.h"
+
+std::string ros_bridge::messages::geometry_msgs::point32::messageType(){
+    return "geometry_msgs/Point32";
+}
+
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 2d0bb2c1f4f1c125a6d435b37323c32d38c6e01c..e02cf27636bcb95bda8c92034635689e8a2af3d5 100644
--- a/src/comm/ros_bridge/include/messages/geometry_msgs/point32.h
+++ b/src/comm/ros_bridge/include/messages/geometry_msgs/point32.h
@@ -12,9 +12,7 @@ namespace geometry_msgs {
 namespace point32 {
 
 
-std::string messageType(){
-    return "geometry_msgs/Point32";
-}
+std::string messageType();
 
 namespace detail {
 using namespace ros_bridge::traits;
diff --git a/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.cpp b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9c67ca377e67afa3873ac57c431f6bea06cd0375
--- /dev/null
+++ b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.cpp
@@ -0,0 +1,6 @@
+#include "polygon.h"
+
+std::string ros_bridge::messages::geometry_msgs::polygon::messageType(){
+    return "geometry_msgs/Polygon";
+}
+
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 f9006a5b9e9f839aecefaa34d6540cda9330d058..2aaf3d393e08c556e75f564fd482a4b5a4f22e59 100644
--- a/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h
+++ b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h
@@ -14,9 +14,7 @@ namespace geometry_msgs {
 namespace polygon {
 
 
-std::string messageType(){
-    return "geometry_msgs/Polygon";
-}
+std::string messageType();
 
 //! @brief C++ representation of geometry_msgs/Polygon
 template <class PointTypeCVR,
diff --git a/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.cpp b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0e974bca76e367642fe9f2d26aa52d0263799cef
--- /dev/null
+++ b/src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.cpp
@@ -0,0 +1,6 @@
+#include "polygon_stamped.h"
+
+std::string ros_bridge::messages::geometry_msgs::polygon_stamped::messageType(){
+    return "geometry_msgs/PolygonStamped";
+}
+
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 f7616fdec6d5cdfdf126db9295484c6a2e68f55a..83868b00ccf58faf5921e7161f2ff318ce3c9156 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
@@ -16,9 +16,7 @@ namespace geometry_msgs {
 namespace polygon_stamped {
 
 
-std::string messageType(){
-    return "geometry_msgs/PolygonStamped";
-}
+std::string messageType();
 
 //! @brief C++ representation of geometry_msgs/PolygonStamped
 template <class PolygonType  = geometry_msgs::polygon::Polygon,
diff --git a/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.cpp b/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..844dc874423d8080446692bc52dec3b953956cd4
--- /dev/null
+++ b/src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.cpp
@@ -0,0 +1,6 @@
+#include "polygon_array.h"
+
+std::string ros_bridge::messages::jsk_recognition_msgs::polygon_array::messageType(){
+    return "jsk_recognition_msgs/PolygonArray";
+}
+
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 9407b0341f4c5589fae55a52a174cfde72d76e74..76693e65b2a675e9ed7922aec4c2981ec951edb6 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
@@ -16,9 +16,7 @@ namespace jsk_recognition_msgs {
 namespace polygon_array {
 
 
-std::string messageType(){
-    return "jsk_recognition_msgs/PolygonArray";
-}
+std::string messageType();
 
 
 //! @brief C++ representation of jsk_recognition_msgs/PolygonArray
@@ -180,10 +178,10 @@ namespace detail {
         value.AddMember("labels", jLabels, allocator);
 
 
-        rapidjson::Value JLikelihood(rapidjson::kArrayType);
+        rapidjson::Value jLikelihood(rapidjson::kArrayType);
         typedef traits::HasMemberLikelihood<PolygonArrayType> HasLikelihood;
-        detail::likelihoodToJson(p, JLikelihood, allocator, traits::Int2Type<HasLikelihood::value>());
-        value.AddMember("likelihood", JLikelihood, allocator);
+        detail::likelihoodToJson(p, jLikelihood, allocator, traits::Int2Type<HasLikelihood::value>());
+        value.AddMember("likelihood", jLikelihood, allocator);
 
         rapidjson::Value jType;
         jType.SetString(messageType().c_str(), allocator);
@@ -248,13 +246,13 @@ bool toJson(const PolygonArrayType &p,
 //! 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, class HeaderType>
+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, Int2Type<HasHeader::value>());
+    return detail::_toJson(p, value, allocator, traits::Int2Type<HasHeader::value>());
 }
 
 
diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.cpp b/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8ba039910c45c4c5f72e511972d130a58dc3c0fc
--- /dev/null
+++ b/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.cpp
@@ -0,0 +1,6 @@
+#include "heartbeat.h"
+
+std::string ros_bridge::messages::nemo_msgs::heartbeat::messageType(){
+    return "nemo_msgs/Heartbeat";
+}
+
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 ce783f4d6a6e2c2710a5a987da51676e48db078d..45c8f5afdb1a4c3ff402434496d4bc0faef50852 100644
--- a/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h
+++ b/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h
@@ -11,9 +11,7 @@ namespace nemo_msgs {
 namespace heartbeat {
 
 
-std::string messageType(){
-    return "nemo_msgs/Heartbeat";
-}
+std::string messageType();
 
 
 //! @brief C++ representation of nemo_msgs/Heartbeat message
diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/progress.cpp b/src/comm/ros_bridge/include/messages/nemo_msgs/progress.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0b408a2df384d229802f0e53cebdab9dcac2677f
--- /dev/null
+++ b/src/comm/ros_bridge/include/messages/nemo_msgs/progress.cpp
@@ -0,0 +1,6 @@
+#include "progress.h"
+
+std::string ros_bridge::messages::nemo_msgs::progress::messageType(){
+    return "nemo_msgs/Progress";
+}
+
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 7825acf906e5f369d7b652e3fa5d5e58e59e4cdf..fa7286c8e93c6f7b35c76544ce3d78da90b28ca8 100644
--- a/src/comm/ros_bridge/include/messages/nemo_msgs/progress.h
+++ b/src/comm/ros_bridge/include/messages/nemo_msgs/progress.h
@@ -14,9 +14,7 @@ namespace nemo_msgs {
 namespace progress {
 
 
-std::string messageType(){
-    return "nemo_msgs/Progress";
-}
+std::string messageType();
 
 //! @brief C++ representation of nemo_msgs/Progress message
 template <class IntType = long, template <class, class...> class ContainterType = std::vector>
diff --git a/src/comm/ros_bridge/include/messages/std_msgs/header.cpp b/src/comm/ros_bridge/include/messages/std_msgs/header.cpp
index 5c108a077b4f1149e39e95d4ee66245eb0860b8e..bd9cc9467423552d7af05778ce22d5899af93a4f 100644
--- a/src/comm/ros_bridge/include/messages/std_msgs/header.cpp
+++ b/src/comm/ros_bridge/include/messages/std_msgs/header.cpp
@@ -63,3 +63,7 @@ void ros_bridge::messages::std_msgs::header::Header::setFrameId(
 {
     _frameId = frameId;
 }
+
+std::string ros_bridge::messages::std_msgs::header::messageType(){
+    return "std_msgs/Header";
+}
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 066f9d4c8a66c3e88705ed9c2fd42ca12c3a4026..bd33f120099c007976117a24ea81f979102c0472 100644
--- a/src/comm/ros_bridge/include/messages/std_msgs/header.h
+++ b/src/comm/ros_bridge/include/messages/std_msgs/header.h
@@ -14,9 +14,7 @@ namespace std_msgs {
 namespace header {
 
 
-std::string messageType(){
-    return "std_msgs/Header";
-}
+std::string messageType();
 
 //! @brief C++ representation of std_msgs/Header
 class Header{
diff --git a/src/comm/ros_bridge/include/messages/std_msgs/time.cpp b/src/comm/ros_bridge/include/messages/std_msgs/time.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7d41fe8ffb5391125401e540497fe4b645f90ee8
--- /dev/null
+++ b/src/comm/ros_bridge/include/messages/std_msgs/time.cpp
@@ -0,0 +1,5 @@
+#include "time.h"
+
+std::string ros_bridge::messages::std_msgs::time::messageType(){
+    return "std_msgs/Time";
+}
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 d88e51c8c133f8b8c87917a7cd797249d44f1893..3160a9743c3797e14fdab877f65f7273f6aed336 100644
--- a/src/comm/ros_bridge/include/messages/std_msgs/time.h
+++ b/src/comm/ros_bridge/include/messages/std_msgs/time.h
@@ -10,9 +10,7 @@ namespace std_msgs {
 //! @brief Namespace containing classes and methodes for std_msgs/Time message generation.
 namespace time {
 
-std::string messageType(){
-    return "std_msgs/Time";
-}
+std::string messageType();
 
 //! @brief C++ representation of std_msgs/Time
 class Time{
diff --git a/src/comm/ros_bridge/include/topic_publisher.cpp b/src/comm/ros_bridge/include/topic_publisher.cpp
index 6ae5f20aff857b849d4ba7e105c5e30d7d15d816..2a705d3429bb09cbc3b92d571868f9f3329fc647 100644
--- a/src/comm/ros_bridge/include/topic_publisher.cpp
+++ b/src/comm/ros_bridge/include/topic_publisher.cpp
@@ -95,9 +95,11 @@ void ros_bridge::com_private::TopicPublisher::publish(
         ros_bridge::com_private::JsonDocUPtr docPtr,
         const char *topic)
 {
-    rapidjson::Value jTopic;
-    jTopic.SetString(topic, docPtr->GetAllocator());
-    docPtr->AddMember("topic", jTopic, docPtr->GetAllocator());
+    std::cout << "TopicPublisher \"topic\": " << topic << std::endl;
+    auto &allocator = docPtr->GetAllocator();
+    rapidjson::Value key("topic", allocator);
+    rapidjson::Value value(topic, allocator);
+    docPtr->AddMember(key, value, allocator);
     std::unique_lock<std::mutex> lock(_mutex);
     _queue.push_back(std::move(docPtr));
     lock.unlock();