trying to pin point the thread issue

qgroundcontrol.pro

@@ -50,7 +50,6 @@ MacBuild {
 
 LinuxBuild {
     CONFIG  += qesp_linux_udev
-    CONFIG  += ccache # improves build time
 }
 
 WindowsBuild {
@@ -494,7 +493,6 @@ HEADERS += \
     src/comm/ros_bridge/include/TopicPublisher.h \
     src/comm/ros_bridge/include/TopicSubscriber.h \
     src/comm/ros_bridge/include/TypeFactory.h \
-    src/comm/ros_bridge/src/PackageBuffer.h \
     src/comm/utilities.h
 SOURCES += \
     src/Snake/clipper/clipper.cpp \
@@ -514,6 +512,7 @@ SOURCES += \
     src/Wima/WimaBridge.cc \
     src/comm/ros_bridge/include/ComPrivateInclude.cpp \
     src/comm/ros_bridge/include/MessageTag.cpp \
+    src/comm/ros_bridge/include/RosBridgeClient.cpp \
     src/comm/ros_bridge/include/Server.cpp \
     src/comm/ros_bridge/include/TopicPublisher.cpp \
     src/comm/ros_bridge/include/TopicSubscriber.cpp \

src/comm/ros_bridge/include/TopicSubscriber.cpp

 #include "TopicSubscriber.h"
 
+#include <thread>
 
 ROSBridge::ComPrivate::TopicSubscriber::TopicSubscriber(CasePacker &casePacker,
         RosbridgeWsClient &rbc) :
@@ -56,37 +57,39 @@ void ROSBridge::ComPrivate::TopicSubscriber::subscribe(
     using namespace std::placeholders;
     auto callbackWrapper = [callback](
             std::shared_ptr<WsClient::Connection>,
-            std::shared_ptr<WsClient::InMessage> in_message){
-                // Parse document.
-                JsonDoc docFull;
-                docFull.Parse(in_message->string().c_str());
-                if ( docFull.HasParseError() ) {
-                    std::cout << "Json document has parse error: "
-                              << in_message->string()
-                              << std::endl;
-                    return;
-                } else if (!docFull.HasMember("msg")) {
-                    std::cout << "Json document does not contain a message (\"msg\"): "
-                              << in_message->string()
-                              << std::endl;
-                    return;
-                }
+            std::shared_ptr<WsClient::InMessage> in_message)
+    {
+        // Parse document.
+        JsonDoc docFull;
+        docFull.Parse(in_message->string().c_str());
+        if ( docFull.HasParseError() ) {
+            std::cout << "Json document has parse error: "
+                      << in_message->string()
+                      << std::endl;
+            return;
+        } else if (!docFull.HasMember("msg")) {
+            std::cout << "Json document does not contain a message (\"msg\"): "
+                      << in_message->string()
+                      << std::endl;
+            return;
+        }
 
-                // Extract message and call callback.
-                JsonDocUPtr pDoc(new JsonDoc());
-                pDoc->CopyFrom(docFull["msg"].Move(), docFull.GetAllocator());
-                callback(std::move(pDoc));
-                return;
+        // Extract message and call callback.
+        JsonDocUPtr pDoc(new JsonDoc());
+        pDoc->CopyFrom(docFull["msg"].Move(), docFull.GetAllocator());
+        callback(std::move(pDoc));
     };
 
     if ( !_rbc.topicAvailable(topic) ){
         // Wait until topic is available.
+        std::cout << "TopicSubscriber: Starting wait thread, " << clientName << std::endl;
         std::thread t([this, clientName, topic, callbackWrapper]{
             this->_rbc.waitForTopic(topic, this->_stopped);
             if ( !this->_stopped->load() ){
                 this->_rbc.addClient(clientName);
                 this->_rbc.subscribe(clientName, topic, callbackWrapper);
-            }
+            }            
+            std::cout << "TopicSubscriber: wait thread end, " << clientName << std::endl;
         });
         t.detach();
     } else {

src/comm/ros_bridge/src/PackageBuffer.h

-#pragma once
-
-#include "boost/lockfree/queue.hpp"
-#include <functional>
-
-namespace ROSBridge {
-namespace Bridge {
-namespace lf = ::boost::lockfree;
-template <class T>
-class PackageBuffer
-{
-public:
-    PackageBuffer();
-
-    void push(T t) {
-        buffer.push(t);
-        if (_pushCallback)
-            _pushCallback();
-    }
-
-    T pop() {
-        T temp = buffer.pop();
-        if (_popCallback)
-            _popCallback();
-        return temp;
-    }
-
-    void setPushCallback(std::function<void(void)> pushCallback) {
-        _pushCallback = pushCallback;
-    }
-
-    void setPopCallback(std::function<void(void)> popCallback) {
-        _popCallback = popCallback;
-    }
-
-    bool empty() const { return buffer.empty();}
-private:
-    lf::queue<T> buffer;
-    std::function<void(void)> _popCallback;
-    std::function<void(void)> _pushCallback;
-};
-
-} // namespace Communicator
-
-} // namespace

src/comm/ros_bridge/src/ROSBridge.cpp

@@ -43,14 +43,22 @@ void ROSBridge::ROSBridge::start()
 
 void ROSBridge::ROSBridge::reset()
 {
+    auto start = std::chrono::high_resolution_clock::now();
     std::cout << "ROSBridge::reset: reset publisher" << std::endl;
     _topicPublisher.reset();
+    std::this_thread::sleep_for(std::chrono::milliseconds(2000));
     std::cout << "ROSBridge::reset: reset subscriber" << std::endl;
     _topicSubscriber.reset();
+    std::this_thread::sleep_for(std::chrono::milliseconds(2000));
     std::cout << "ROSBridge::reset: reset server" << std::endl;
     _server.reset();
+    std::this_thread::sleep_for(std::chrono::milliseconds(2000));
     std::cout << "ROSBridge::reset: _stopped->store(true)" << std::endl;
     _stopped->store(true);
+    std::this_thread::sleep_for(std::chrono::milliseconds(2000));
+    auto end = std::chrono::high_resolution_clock::now();
+    auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
+    std::cout << "ROSBridge reset duration: " << delta << " ms" << std::endl;
 }
 
 bool ROSBridge::ROSBridge::ping()