bug appearded again

src/Wima/WimaController.cc

@@ -719,27 +719,27 @@ void WimaController::_eventTimerHandler()
 
 
         auto start = std::chrono::high_resolution_clock::now();
-        _pRosBridge->publish(_snakeTilesLocal, "/snake/tiles");
         _pRosBridge->publish(_snakeOrigin, "/snake/origin");
+        _pRosBridge->publish(_snakeTilesLocal, "/snake/tiles");
 
-        using namespace std::placeholders;
-        auto progressCallBack = std::bind(&WimaController::_progressFromJson,
-                                          this,
-                                          _1,
-                                          std::ref(_nemoProgress));
-        _pRosBridge->subscribe("/nemo/progress", progressCallBack);
-        _pRosBridge->subscribe("/nemo/heartbeat", [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();
-        });
+//        using namespace std::placeholders;
+//        auto progressCallBack = std::bind(&WimaController::_progressFromJson,
+//                                          this,
+//                                          _1,
+//                                          std::ref(_nemoProgress));
+//        _pRosBridge->subscribe("/nemo/progress", progressCallBack);
+//        _pRosBridge->subscribe("/nemo/heartbeat", [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 end = std::chrono::high_resolution_clock::now();
         qWarning() << "Duration: " << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()

src/comm/ros_bridge/include/TopicPublisher.cpp

@@ -37,11 +37,6 @@ void ROSBridge::ComPrivate::TopicPublisher::start()
     if ( _running.load() )  // start called while thread running.
         return;
     _running.store(true);
-    {
-        std::lock_guard<std::mutex> lk(*_rbcMutex);
-        _rbc->addClient(ROSBridge::ComPrivate::_topicAdvertiserKey);
-        _rbc->addClient(ROSBridge::ComPrivate::_topicPublisherKey);
-    }
     ROSBridge::ComPrivate::ThreadData data{
                 *_casePacker,
                 *_rbc,
@@ -64,17 +59,19 @@ void ROSBridge::ComPrivate::TopicPublisher::reset()
     if ( !_pThread )
         return;
     _pThread->join();
-    {
-        std::lock_guard<std::mutex> lk(*_rbcMutex);
-        _rbc->removeClient(ROSBridge::ComPrivate::_topicAdvertiserKey);
-        _rbc->removeClient(ROSBridge::ComPrivate::_topicPublisherKey);
-    }
     _queue.clear();
     _advertisedTopicsHashList.clear();
 }
 
 void ROSBridge::ComPrivate::transmittLoop(ROSBridge::ComPrivate::ThreadData data)
 {
+    // Init.
+    {
+        std::lock_guard<std::mutex> lk(data.rbcMutex);
+        data.rbc.addClient(ROSBridge::ComPrivate::_topicAdvertiserKey);
+        data.rbc.addClient(ROSBridge::ComPrivate::_topicPublisherKey);
+    }
+    // Main Loop.
     while(data.running.load()){
         std::unique_lock<std::mutex> lk(data.queueMutex);
         // Check if new data available, wait if not.
@@ -87,13 +84,6 @@ void ROSBridge::ComPrivate::transmittLoop(ROSBridge::ComPrivate::ThreadData data
         data.queue.pop_front();
         lk.unlock();
 
-        // Debug output.
-//        std::cout << "Transmitter loop json document:" << std::endl;
-//        rapidjson::OStreamWrapper out(std::cout);
-//        rapidjson::Writer<rapidjson::OStreamWrapper> writer(out);
-//        pJsonDoc->Accept(writer);
-//        std::cout << std::endl << std::endl;
-
         // Get tag from Json message and remove it.
         Tag tag;
         bool ret = data.casePacker.getTag(pJsonDoc, tag);
@@ -108,20 +98,28 @@ void ROSBridge::ComPrivate::transmittLoop(ROSBridge::ComPrivate::ThreadData data
         if ( data.advertisedTopicsHashList.count(hash) == 0) {
             data.advertisedTopicsHashList.insert(hash);
             {
+                std::cout << ROSBridge::ComPrivate::_topicAdvertiserKey << ";"
+                          << tag.topic() << ";"
+                          << tag.messageType() << ";" << std::endl;
                 std::lock_guard<std::mutex> lk(data.rbcMutex);
                 data.rbc.advertise(ROSBridge::ComPrivate::_topicAdvertiserKey,
                                    tag.topic(),
                                    tag.messageType() );
             }
         }
-        // Debug output.
-        //std::cout << "Hash Set size: " << advertisedTopicsHashList.size() << std::endl;
 
-        // Send Json message.
+        // Publish Json message.
         {
             std::lock_guard<std::mutex> lk(data.rbcMutex);
             data.rbc.publish(tag.topic(), *pJsonDoc.get());
         }
     } // while loop
+
+    // Tidy up.
+    {
+        std::lock_guard<std::mutex> lk(data.rbcMutex);
+        data.rbc.removeClient(ROSBridge::ComPrivate::_topicAdvertiserKey);
+        data.rbc.removeClient(ROSBridge::ComPrivate::_topicPublisherKey);
+    }
 }
 

src/comm/ros_bridge/include/TopicPublisher.h

@@ -36,11 +36,6 @@ public:
     //! @brief Resets the publisher.
     void reset();
 
-    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){
         {
         std::lock_guard<std::mutex> lock(_queueMutex);
@@ -49,6 +44,12 @@ public:
         _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));
+    }
+
 private:
     JsonQueue           _queue;
     std::mutex          _queueMutex;

src/comm/ros_bridge/include/TopicSubscriber.cpp

@@ -24,13 +24,13 @@ void ROSBridge::ComPrivate::TopicSubscriber::start()
 void ROSBridge::ComPrivate::TopicSubscriber::reset()
 {
     if ( _running ){
+        _running = false;
         {
             std::lock_guard<std::mutex> lk(*_rbcMutex);
             for (std::string clientName : _clientList){
                 _rbc->removeClient(clientName);
             }
         }
-        _running = false;
         {
             std::lock_guard<std::mutex> lk(_callbackMap.mutex);
             _callbackMap.map.clear();