diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index 274f0c0a49f57315cd4f973ebabd62eb12149a6c..2f62555ae92d40e738222b94acd03f5f6fbffcec 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -27,7 +27,7 @@ QGCROOT  = $$PWD
 
 DebugBuild {
     DESTDIR  = $${OUT_PWD}/debug
-    #DEFINES += DEBUG
+    DEFINES += DEBUG
     #DEFINES += ROS_BRIDGE_CLIENT_DEBUG
 }
 else {
@@ -50,6 +50,7 @@ MacBuild {
 
 LinuxBuild {
     CONFIG  += qesp_linux_udev
+    CONFIG  += ccache # improves build time
 }
 
 WindowsBuild {
diff --git a/src/Wima/WaypointManager/Slicer.cpp b/src/Wima/WaypointManager/Slicer.cpp
index a880f52258536244fffc6e362c7344e1d0754dcf..6b13a0e019f1eb579f4edd2800a91054c13af6a3 100644
--- a/src/Wima/WaypointManager/Slicer.cpp
+++ b/src/Wima/WaypointManager/Slicer.cpp
@@ -43,16 +43,15 @@ void Slicer::_updateIdx(long size)
 {
     _overlap = _overlap < _N ? _overlap : _N-1;
 
-    long maxStart  = size-_N;
-    _idxStart = _idxStart <= maxStart   ? _idxStart : maxStart;
-    _idxStart = _idxStart < 0           ? 0         : _idxStart;
+    _idxStart = _idxStart < size   ? _idxStart : size-1;
+    _idxStart = _idxStart < 0      ? 0         : _idxStart;
 
     _idxEnd = _idxStart + _N - 1;
     _idxEnd = _idxEnd < size ? _idxEnd : size-1;
 
-    _idxNext = _idxEnd + 1 - _overlap;
-    _idxNext = _idxNext < 0       ? 0        : _idxNext;
-    _idxNext = _idxNext < size    ? _idxNext : size-1;
+    _idxNext = _idxEnd == size -1 ? _idxStart : _idxEnd + 1 - _overlap;
+    _idxNext = _idxNext < 0       ? 0         : _idxNext;
+    _idxNext = _idxNext < size    ? _idxNext  : size-1;
 
     _idxPrevious = _idxStart + _overlap - _N;
     _idxPrevious = _idxPrevious < 0       ? 0            : _idxPrevious;
diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc
index 4da4157d928bcfd38bbc420644432376b3dc911f..2a319e15ceab52a63d7a4f0867575a6460ed2c76 100644
--- a/src/Wima/WimaController.cc
+++ b/src/Wima/WimaController.cc
@@ -80,9 +80,11 @@ WimaController::WimaController(QObject *parent)
     , _snakeMinTransectLength   (settingsGroup, _metaDataMap[snakeMinTransectLengthName])
     , _nemoHeartbeat            (0 /*status: not connected*/)
     , _fallbackStatus           (0 /*status: not connected*/)
-    , _bridgeSetupDone          (false)
     , _pRosBridge               (new ROSBridge::ROSBridge())
-    , _pCasePacker               (_pRosBridge->casePacker())
+    , _pCasePacker              (_pRosBridge->casePacker())
+    , _batteryLevelTicker       (EVENT_TIMER_INTERVAL, 1000 /*ms*/)
+    , _snakeTicker              (EVENT_TIMER_INTERVAL, 200 /*ms*/)
+    , _nemoTimeoutTicker        (EVENT_TIMER_INTERVAL, 5000 /*ms*/)
 {
     // Set up facts.
     _showAllMissionItems.setRawValue(true);
@@ -120,8 +122,6 @@ WimaController::WimaController(QObject *parent)
     connect(this, &QObject::destroyed, &this->_snakeWorker, &SnakeWorker::quit);
 
     // Snake.
-    connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_startStopRosBridge);
-    _startStopRosBridge();
     connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_initStartSnakeWorker);
     _initStartSnakeWorker();
     connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_switchSnakeManager);
@@ -701,13 +701,9 @@ void WimaController::_checkBatteryLevel()
 
 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() )
+    if ( enableLowBatteryHandling->rawValue().toBool() && _batteryLevelTicker.ready() )
         _checkBatteryLevel();
 
     // Snake flight plan update necessary?
@@ -716,78 +712,25 @@ void WimaController::_eventTimerHandler()
 //        }
 //    }
 
-    if ( nemoStatusTicker.ready() ) {
+    if ( _nemoTimeoutTicker.ready() && _enableSnake.rawValue().toBool() ) {
         this->_nemoHeartbeat.setStatus(_fallbackStatus);
         emit WimaController::nemoStatusChanged();
         emit WimaController::nemoStatusStringChanged();
     }
 
-    if ( snakeTicker.ready() ) {
-        if ( _enableSnake.rawValue().toBool()
-             && _pRosBridge->connected() ) {
-            if ( !_bridgeSetupDone ) {
-
-                _pRosBridge->start();
-                _pRosBridge->publish(_snakeOrigin, "/snake/origin");
-                _pRosBridge->publish(_snakeTilesLocal, "/snake/tiles");
-                _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();
-                });
-                _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();
-                });
-
-                auto pOrigin = &_snakeOrigin;
-                auto casePacker = _pCasePacker;
-                _pRosBridge->advertiseService("/snake/get_origin", "snake_msgs/GetOrigin",
-                                              [casePacker, pOrigin](JsonDocUPtr) -> JsonDocUPtr {
-                    JsonDocUPtr pDoc = casePacker->pack(*pOrigin, "");
-                    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;
-
-                });
-
-                auto pTiles = &_snakeTilesLocal;
-                _pRosBridge->advertiseService("/snake/get_tiles", "snake_msgs/GetTiles",
-                                              [casePacker, pTiles](JsonDocUPtr) -> JsonDocUPtr {
-
-                    JsonDocUPtr pDoc =  casePacker->pack(*pTiles, "");
-                    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;
-
-                });
-                _bridgeSetupDone = true;
+    if ( _snakeTicker.ready() ) {
+        if ( _enableSnake.rawValue().toBool() ) {
+            if ( !_pRosBridge->isRunning() && _pRosBridge->ping() ) {
+                _setupTopicService();
+            }
+
+            if ( !_pRosBridge->ping() ){
+                _pRosBridge->reset();
             }
-        } else if ( _bridgeSetupDone) {
-            _pRosBridge->reset();
-            _bridgeSetupDone = false;
+
+        } else {
+            if ( _pRosBridge->isRunning() )
+                _pRosBridge->reset();
         }
     }
 }
@@ -933,15 +876,6 @@ void WimaController::_snakeStoreWorkerResults()
     qWarning() << "WimaController::_snakeStoreWorkerResults execution time: " << duration << " ms.";
 }
 
-void WimaController::_startStopRosBridge()
-{
-    if ( _enableSnake.rawValue().toBool() ) {
-        _pRosBridge->start();
-    } else {
-        _pRosBridge->reset();
-    }
-}
-
 void WimaController::_initStartSnakeWorker()
 {
     if ( !_enableSnake.rawValue().toBool() )
@@ -971,3 +905,63 @@ void WimaController::_switchSnakeManager(QVariant variant)
         _switchWaypointManager(_defaultManager);
     }
 }
+
+void WimaController::_setupTopicService()
+{
+    _pRosBridge->start();
+    _pRosBridge->publish(_snakeOrigin, "/snake/origin");
+    _pRosBridge->publish(_snakeTilesLocal, "/snake/tiles");
+    _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();
+    });
+    _pRosBridge->subscribe("/nemo/heartbeat",
+                           /* callback */ [this](JsonDocUPtr pDoc){
+        if ( !this->_pRosBridge->casePacker()->unpack(pDoc, this->_nemoHeartbeat) ) {
+            if ( this->_nemoHeartbeat.status() == this->_fallbackStatus )
+                return;
+            this->_nemoHeartbeat.setStatus(this->_fallbackStatus);
+        }
+
+        this->_nemoTimeoutTicker.reset();
+        this->_fallbackStatus = -1; /*Timeout*/
+        emit WimaController::nemoStatusChanged();
+        emit WimaController::nemoStatusStringChanged();
+    });
+
+    auto pOrigin = &_snakeOrigin;
+    auto casePacker = _pCasePacker;
+    _pRosBridge->advertiseService("/snake/get_origin", "snake_msgs/GetOrigin",
+                                  [casePacker, pOrigin](JsonDocUPtr) -> JsonDocUPtr {
+        JsonDocUPtr pDoc = casePacker->pack(*pOrigin, "");
+        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;
+
+    });
+
+    auto pTiles = &_snakeTilesLocal;
+    _pRosBridge->advertiseService("/snake/get_tiles", "snake_msgs/GetTiles",
+                                  [casePacker, pTiles](JsonDocUPtr) -> JsonDocUPtr {
+
+        JsonDocUPtr pDoc =  casePacker->pack(*pTiles, "");
+        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;
+
+    });
+}
diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h
index e39c95321245c37f2f6c156dae04afc910e6590d..3c40cfd6d7a1a5cd3a98fa3643abd3157929b0c5 100644
--- a/src/Wima/WimaController.h
+++ b/src/Wima/WimaController.h
@@ -43,11 +43,9 @@
 
 #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;
@@ -340,9 +338,9 @@ private slots:
     // Snake.
     void _setSnakeCalcInProgress             (bool inProgress);
     void _snakeStoreWorkerResults            ();
-    void _startStopRosBridge                 ();
     void _initStartSnakeWorker               ();
     void _switchSnakeManager                 (QVariant variant);
+    void _setupTopicService                     ();
     // Periodic tasks.
     void _eventTimerHandler                  (void);
     // Waypoint manager handling.
@@ -415,11 +413,13 @@ private:
     int                     _fallbackStatus;
     ROSBridgePtr            _pRosBridge;
     const CasePacker        *_pCasePacker;
-    bool                    _bridgeSetupDone;
     static StatusMap        _nemoStatusMap;
 
     // Periodic tasks.
-    QTimer                  _eventTimer;
+    QTimer      _eventTimer;
+    EventTicker _batteryLevelTicker;
+    EventTicker _snakeTicker;
+    EventTicker _nemoTimeoutTicker;
 
 };
 
diff --git a/src/comm/ros_bridge/include/ROSBridge.h b/src/comm/ros_bridge/include/ROSBridge.h
index 49e3ec4973068c8a0142341a08dd38b28e24082a..d1b50d05aa2ffff4be8cf0e8bcc51d57b3748b4b 100644
--- a/src/comm/ros_bridge/include/ROSBridge.h
+++ b/src/comm/ros_bridge/include/ROSBridge.h
@@ -40,9 +40,12 @@ public:
     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.
-    bool connected();
+    bool ping();
+
+    bool isRunning();
 
 private:
+    bool                    _running;
     TypeFactory             _typeFactory;
     JsonFactory             _jsonFactory;
     CasePacker              _casePacker;
diff --git a/src/comm/ros_bridge/include/RosBridgeClient.h b/src/comm/ros_bridge/include/RosBridgeClient.h
index d28fda0fa1495dd02704bf7f36694166d266368e..3d4ac6c1d3a1450cfa947e82711cd2f44bd2e470 100644
--- a/src/comm/ros_bridge/include/RosBridgeClient.h
+++ b/src/comm/ros_bridge/include/RosBridgeClient.h
@@ -16,15 +16,36 @@
 #include <thread>
 #include <future>
 #include <mutex>
+#include <tuple>
+#include <deque>
 
 using WsClient = SimpleWeb::SocketClient<SimpleWeb::WS>;
 using InMessage =  std::function<void(std::shared_ptr<WsClient::Connection>, std::shared_ptr<WsClient::InMessage>)>;
 
+template <typename T>
+constexpr typename std::underlying_type<T>::type integral(T value)
+{
+    return static_cast<typename std::underlying_type<T>::type>(value);
+}
+
 class RosbridgeWsClient
 {
+  using TopicData = std::tuple<std::string /*topic*/,
+                               std::string /*client name*/,
+                               std::shared_ptr<WsClient> /*client*/,
+                               std::shared_ptr<std::atomic_bool> /*topic ready*/>;
+  enum class TopicEnum{
+      TopicName = 0,
+      ClientName = 1,
+      Client = 2,
+      Ready = 3
+  };
+
   std::string server_port_path;
-  std::unordered_map<std::string, std::shared_ptr<WsClient>> client_map;
-  std::mutex map_mutex;
+  std::unordered_map<std::string /*client name*/,
+                     std::shared_ptr<WsClient> /*client*/> client_map;
+  std::deque<TopicData> advertised_topics_list;
+  std::mutex mutex;
 
   void start(const std::string& client_name, std::shared_ptr<WsClient> client, const std::string& message)
   {
@@ -66,8 +87,8 @@ class RosbridgeWsClient
         std::cout << client_name << ": Error: " << ec << ", error message: " << ec.message() << std::endl;
       };
     }
-#endif
 
+#endif
 #ifdef DEBUG
     std::thread client_thread([client_name, client]() {
 #else
@@ -95,11 +116,13 @@ public:
 
   ~RosbridgeWsClient()
   {
-    std::lock_guard<std::mutex> lk(map_mutex); // neccessary?
+    for (auto& topicData : advertised_topics_list){
+        unadvertise(std::get<integral(TopicEnum::ClientName)>(topicData),
+                    std::get<integral(TopicEnum::TopicName)>(topicData));
+    }
     for (auto& client : client_map)
     {
-      client.second->stop();
-      client.second.reset();
+      removeClient(client.first);
     }
   }
 
@@ -133,7 +156,7 @@ public:
   // Adds a client to the client_map.
   void addClient(const std::string& client_name)
   {
-    std::lock_guard<std::mutex> lk(map_mutex);
+    std::lock_guard<std::mutex> lk(mutex);
     std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
     if (it == client_map.end())
     {
@@ -149,7 +172,7 @@ public:
 
   std::shared_ptr<WsClient> getClient(const std::string& client_name)
   {
-    std::lock_guard<std::mutex> lk(map_mutex);
+    std::lock_guard<std::mutex> lk(mutex);
     std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
     if (it != client_map.end())
     {
@@ -160,7 +183,7 @@ public:
 
   void stopClient(const std::string& client_name)
   {
-    std::lock_guard<std::mutex> lk(map_mutex);
+    std::lock_guard<std::mutex> lk(mutex);
     std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
     if (it != client_map.end())
     {
@@ -183,23 +206,26 @@ public:
 
   void removeClient(const std::string& client_name)
   {
-    std::lock_guard<std::mutex> lk(map_mutex);
+    std::lock_guard<std::mutex> lk(mutex);
     std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
     if (it != client_map.end())
     {
       // Stop the client asynchronously in 100 ms.
       // This is to ensure, that all threads involving the client have been launched.
-      std::thread t([](std::shared_ptr<WsClient> client){
-         std::this_thread::sleep_for(std::chrono::milliseconds(100));
-         client->stop();
-         client.reset();
-      },
-      it->second /*lambda param*/ );
-      client_map.erase(it);
-      t.detach();
+      std::shared_ptr<WsClient> client = it->second;
+#ifdef DEBUG
+      std::thread t([client, client_name](){
+#else
+      std::thread t([client](){
+#endif
+          std::this_thread::sleep_for(std::chrono::milliseconds(100));
+          client->stop();
 #ifdef DEBUG
       std::cout << client_name << " has been removed" << std::endl;
 #endif
+      });
+      client_map.erase(it);
+      t.detach();
     }
 #ifdef DEBUG
     else
@@ -213,19 +239,51 @@ public:
   // One client per topic!
   void advertise(const std::string& client_name, const std::string& topic, const std::string& type, const std::string& id = "")
   {
-    std::lock_guard<std::mutex> lk(map_mutex);
-    std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
-    if (it != client_map.end())
+    std::lock_guard<std::mutex> lk(mutex);
+    std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it_client = client_map.find(client_name);
+    if (it_client != client_map.end())
     {
-      std::string message = "\"op\":\"advertise\", \"topic\":\"" + topic + "\", \"type\":\"" + type + "\"";
+      auto it_topic = std::find_if(advertised_topics_list.begin(),
+                                   advertised_topics_list.end(),
+                                   [topic](const TopicData &td){
+          return topic == std::get<integral(TopicEnum::TopicName)>(td);
+      });
+      if ( it_topic != advertised_topics_list.end()){
+#ifdef DEBUG
+            std::cerr << "topic: " << topic << " already advertised" << std::endl;
+#endif
+            return;
+      }
+      auto client = it_client->second;
+      auto ready = std::make_shared<std::atomic_bool>(false);
+      advertised_topics_list.push_back(std::make_tuple(topic, client_name, client, ready));
 
+      std::string message = "\"op\":\"advertise\", \"topic\":\"" + topic + "\", \"type\":\"" + type + "\"";
       if (id.compare("") != 0)
       {
         message += ", \"id\":\"" + id + "\"";
       }
       message = "{" + message + "}";
 
-      start(client_name, it->second, message);
+#ifdef DEBUG
+      client->on_open = [client_name, message, ready](std::shared_ptr<WsClient::Connection> connection) {
+#else
+      client->on_open = [message, ready](std::shared_ptr<WsClient::Connection> connection) {
+#endif
+
+#ifdef DEBUG
+        std::cout << client_name << ": Opened connection" << std::endl;
+        std::cout << client_name << ": Sending message: " << message << std::endl;
+#endif
+        connection->send(message);
+        // Wait for rosbridge_server to process the request and mark topic as "ready".
+        // This could be avoided by waiting for a status message. However, at the time of
+        // writing rosbridge_server status messages are still experimental.
+        std::this_thread::sleep_for(std::chrono::milliseconds(500));
+        ready->store(true);
+      };
+
+      start(client_name, client, message);
     }
 #ifdef DEBUG
     else
@@ -235,12 +293,83 @@ public:
 #endif
   }
 
+  void unadvertise(const std::string& client_name,
+                   const std::string& topic,
+                   const std::string& id = ""){
+      std::lock_guard<std::mutex> lk(mutex);
+      std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it_client = client_map.find(client_name);
+      if (it_client != client_map.end())
+      {
+          // Topic advertised?
+          auto it_topic = std::find_if(advertised_topics_list.begin(),
+                                       advertised_topics_list.end(),
+                                       [topic](const TopicData &td){
+              return topic == std::get<integral(TopicEnum::TopicName)>(td);
+          });
+        if ( it_topic == advertised_topics_list.end()){
+  #ifdef DEBUG
+              std::cerr << "topic: " << topic << " not advertised" << std::endl;
+  #endif
+              return;
+        }
+
+        std::string message = "\"op\":\"unadvertise\"";
+        if (id.compare("") != 0)
+        {
+          message += ", \"id\":\"" + id + "\"";
+        }
+        message += ", \"topic\":\"" + topic + "\"";
+        message = "{" + message + "}";
+
+        auto client = it_client->second;
+        auto ready = std::get<integral(TopicEnum::Ready)>(*it_topic);
+  #ifdef DEBUG
+        client->on_open = [client_name, message, ready](std::shared_ptr<WsClient::Connection> connection) {
+  #else
+        client->on_open = [message, ready](std::shared_ptr<WsClient::Connection> connection) {
+  #endif
+
+  #ifdef DEBUG
+          std::cout << client_name << ": Opened connection" << std::endl;
+          std::cout << client_name << ": Sending message: " << message << std::endl;
+  #endif
+          while ( !ready->load() ){ // Wait for topic to be advertised.
+              std::this_thread::sleep_for(std::chrono::milliseconds(500));
+          }
+          connection->send(message);
+        };
+
+        start(client_name, client, message);
+        advertised_topics_list.erase(it_topic);
+      }
+  #ifdef DEBUG
+      else
+      {
+        std::cerr << client_name << "has not been created" << std::endl;
+      }
+  #endif
+
+  }
+
   void publish(const std::string& topic, const rapidjson::Document& msg, const std::string& id = "")
   {
+    std::lock_guard<std::mutex> lk(mutex);
+    auto it_topic = std::find_if(advertised_topics_list.begin(),
+                                 advertised_topics_list.end(),
+                                 [&topic](const TopicData &td){
+        return topic == std::get<integral(TopicEnum::TopicName)>(td);
+    });
+    if ( it_topic == advertised_topics_list.end() ){
+#ifdef DEBUG
+        std::cerr << "topic: " << topic << " not yet advertised" << std::endl;
+#endif
+        return;
+    }
     rapidjson::StringBuffer strbuf;
     rapidjson::Writer<rapidjson::StringBuffer> writer(strbuf);
     msg.Accept(writer);
 
+    std::string client_name = "publish_client" + topic;
     std::string message = "\"op\":\"publish\", \"topic\":\"" + topic + "\", \"msg\":" + strbuf.GetString();
 
     if (id.compare("") != 0)
@@ -250,24 +379,28 @@ public:
     message = "{" + message + "}";
 
     std::shared_ptr<WsClient> publish_client = std::make_shared<WsClient>(server_port_path);
-
-    publish_client->on_open = [message](std::shared_ptr<WsClient::Connection> connection) {
+    auto ready = std::get<integral(TopicEnum::Ready)>(*it_topic);
+    publish_client->on_open = [message, client_name, ready](std::shared_ptr<WsClient::Connection> connection) {
 #ifdef DEBUG
-      std::cout << "publish_client: Opened connection" << std::endl;
-      std::cout << "publish_client: Sending message: " << message << std::endl;
+      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;
 #endif
+      while ( !ready->load() ){ // Wait for the topic to be advertised.
+          std::this_thread::sleep_for(std::chrono::milliseconds(500));
+      }
       connection->send(message);
 
       // TODO: This could be improved by creating a thread to keep publishing the message instead of closing it right away
       connection->send_close(1000);
     };
 
-    start("publish_client", publish_client, message);
+    start(client_name, publish_client, message);
   }
 
   void subscribe(const std::string& client_name, const std::string& topic, const InMessage& callback, const std::string& id = "", const std::string& type = "", int throttle_rate = -1, int queue_length = -1, int fragment_size = -1, const std::string& compression = "")
   {
-    std::lock_guard<std::mutex> lk(map_mutex);
+    std::lock_guard<std::mutex> lk(mutex);
     std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
     if (it != client_map.end())
     {
@@ -312,7 +445,7 @@ public:
 
   void advertiseService(const std::string& client_name, const std::string& service, const std::string& type, const InMessage& callback)
   {
-    std::lock_guard<std::mutex> lk(map_mutex);
+    std::lock_guard<std::mutex> lk(mutex);
     std::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
     if (it != client_map.end())
     {
diff --git a/src/comm/ros_bridge/include/Server.cpp b/src/comm/ros_bridge/include/Server.cpp
index abfb4b1ba0d353037c821fa1c7c23cebb8809f4e..8341f78739429102f1c5e3fcb43db6f61e1092a9 100644
--- a/src/comm/ros_bridge/include/Server.cpp
+++ b/src/comm/ros_bridge/include/Server.cpp
@@ -39,9 +39,20 @@ void ROSBridge::ComPrivate::Server::advertiseService(const std::string &service,
           return;
         }
 
-        // Extract message and call callback.
         if ( !document.HasMember("args") || !document["args"].IsObject()){
-            std::cerr << "advertiseServiceCallback(): message has no args: "
+            std::cerr << "advertiseServiceCallback(): message has no args member: "
+                      << messagebuf << std::endl;
+            return;
+        }
+
+        if ( !document.HasMember("service") || !document["service"].IsString()){
+            std::cerr << "advertiseServiceCallback(): message has no service member: "
+                      << messagebuf << std::endl;
+            return;
+        }
+
+        if ( !document.HasMember("id") || !document["id"].IsString()){
+            std::cerr << "advertiseServiceCallback(): message has no id member: "
                       << messagebuf << std::endl;
             return;
         }
@@ -51,17 +62,10 @@ void ROSBridge::ComPrivate::Server::advertiseService(const std::string &service,
             pDoc->CopyFrom(document["args"].Move(), document.GetAllocator());
         JsonDocUPtr pDocResponse = userCallback(std::move(pDoc));
 
-        rapidjson::OStreamWrapper out(std::cout);
-        // Write document...
-        rapidjson::Writer<rapidjson::OStreamWrapper> writer(out);
-        std::cout << "Ros Server: ";
-        (*pDocResponse).Accept(writer);
-        std::cout << std::endl;
-
         rbc->serviceResponse(
                     document["service"].GetString(),
                     document["id"].GetString(),
-                    true,
+                    pDocResponse->MemberCount() > 0 ? true : false,
                     *pDocResponse);
 
         return;
diff --git a/src/comm/ros_bridge/include/TopicPublisher.cpp b/src/comm/ros_bridge/include/TopicPublisher.cpp
index 1e44a7a74804f465ca7f2f9365dc450bb26d664e..687be18d3be9b096d1556b29ba87631ba29f470e 100644
--- a/src/comm/ros_bridge/include/TopicPublisher.cpp
+++ b/src/comm/ros_bridge/include/TopicPublisher.cpp
@@ -89,9 +89,6 @@ void ROSBridge::ComPrivate::transmittLoop(ROSBridge::ComPrivate::ThreadData data
         auto it = clientMap.find(hash);
         if ( it == clientMap.end()) { // Need to advertise topic?
             clientMap.insert(std::make_pair(hash, clientName));
-            std::cout << clientName << ";"
-                      << tag.topic() << ";"
-                      << tag.messageType() << ";" << std::endl;
             data.rbc.addClient(clientName);
             data.rbc.advertise(clientName,
                                tag.topic(),
diff --git a/src/comm/ros_bridge/src/ROSBridge.cpp b/src/comm/ros_bridge/src/ROSBridge.cpp
index f552faf9d974396bba32200c564188c12c0642d3..adac201da53bd77c40e6dd4cdb4ff3a0a4e17576 100644
--- a/src/comm/ros_bridge/src/ROSBridge.cpp
+++ b/src/comm/ros_bridge/src/ROSBridge.cpp
@@ -1,7 +1,8 @@
 #include "ros_bridge/include/ROSBridge.h"
 
 ROSBridge::ROSBridge::ROSBridge() :
-    _casePacker(&_typeFactory, &_jsonFactory)
+    _running(false)
+  ,  _casePacker(&_typeFactory, &_jsonFactory)
   , _rbc("localhost:9090")
   , _topicPublisher(_casePacker, _rbc)
   , _topicSubscriber(_casePacker, _rbc)
@@ -37,6 +38,7 @@ void ROSBridge::ROSBridge::start()
     _topicPublisher.start();
     _topicSubscriber.start();
     _server.start();
+    _running = true;
 }
 
 void ROSBridge::ROSBridge::reset()
@@ -44,10 +46,16 @@ void ROSBridge::ROSBridge::reset()
     _topicPublisher.reset();
     _topicSubscriber.reset();
     _server.reset();
+    _running = false;
 }
 
-bool ROSBridge::ROSBridge::connected()
+bool ROSBridge::ROSBridge::ping()
 {
     return _rbc.connected();
 }
 
+bool ROSBridge::ROSBridge::isRunning()
+{
+    return _running;
+}
+