diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index f3f0ceabe8cccde436da76a9cd521922717a7f5c..c3325f6cee547639c841cb6537488bad16f7385b 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -28,12 +28,11 @@ QGCROOT  = $$PWD
 DebugBuild {
     DESTDIR  = $${OUT_PWD}/debug
     DEFINES += DEBUG
-    DEFINES += ROS_BRIDGE_DEBUG
-    #DEFINES += ROS_BRIDGE_CLIENT_DEBUG
+    #DEFINES += ROS_BRIDGE_DEBUG
 }
 else {
     DESTDIR  = $${OUT_PWD}/release
-    DEFINES += ROS_BRIDGE_DEBUG
+    #DEFINES += ROS_BRIDGE_DEBUG
     DEFINES += NDEBUG
 }
 
diff --git a/src/comm/ros_bridge/include/RosBridgeClient.cpp b/src/comm/ros_bridge/include/RosBridgeClient.cpp
index b423b1abc7468633da413e145b961195aa88b03d..3a44e7766a88c6420ff8e98b7f1b315db8557426 100644
--- a/src/comm/ros_bridge/include/RosBridgeClient.cpp
+++ b/src/comm/ros_bridge/include/RosBridgeClient.cpp
@@ -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 ROS_BRIDGE_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 ROS_BRIDGE_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 ROS_BRIDGE_DEBUG
-                            std::cout << "status_client opened" << std::endl;
-#endif
                             this->isConnected->store(true);
                             status_set->store(true);
                     };
@@ -190,9 +180,6 @@ void RosbridgeWsClient::run()
                     });
                     if ( topics_it == task_list.end() ){
                         // Call /rosapi/topics service.
-#ifdef ROS_BRIDGE_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,
@@ -238,9 +225,6 @@ void RosbridgeWsClient::run()
                     });
                     if ( services_it == task_list.end() ){
                         // Call /rosapi/services service.
-#ifdef ROS_BRIDGE_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,
diff --git a/src/comm/ros_bridge/include/com_private.cpp b/src/comm/ros_bridge/include/com_private.cpp
index 2dceb9ee908e5c3d64fb36bec64440730d658e20..8f61791d5e037ba30d2ef968807800599d0f49e7 100644
--- a/src/comm/ros_bridge/include/com_private.cpp
+++ b/src/comm/ros_bridge/include/com_private.cpp
@@ -19,11 +19,7 @@ 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() ){
-
-        rapidjson::StringBuffer sb;
-        rapidjson::Writer<rapidjson::StringBuffer> writer(sb);
-        doc["topic"].Accept(writer);
-        topic = sb.GetString();
+        topic = doc["topic"].GetString();
         return true;
     } else
         return false;
@@ -41,10 +37,7 @@ 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() ){
-        rapidjson::StringBuffer sb;
-        rapidjson::Writer<rapidjson::StringBuffer> writer(sb);
-        doc["type"].Accept(writer);
-        type = sb.GetString();
+        type = doc["type"].GetString();
         return true;
     } else
         return false;
diff --git a/src/comm/ros_bridge/include/topic_publisher.cpp b/src/comm/ros_bridge/include/topic_publisher.cpp
index e9e9ce2b200d215f76a512a3c27912c4006b2fae..dbee9d683c8b34f3edc599296991bf6c417ea23b 100644
--- a/src/comm/ros_bridge/include/topic_publisher.cpp
+++ b/src/comm/ros_bridge/include/topic_publisher.cpp
@@ -39,7 +39,17 @@ void ros_bridge::com_private::TopicPublisher::start()
             std::string topic;
             bool ret = com_private::getTopic(*pJsonDoc, topic);
             assert(ret);
-            (void)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;