thread issue seems to be solved

src/Wima/WimaController.cc

@@ -724,16 +724,12 @@ void WimaController::_eventTimerHandler()
 
     if ( _snakeTicker.ready() ) {
         if ( _enableSnake.rawValue().toBool() ) {
-            if ( !_pRosBridge->isRunning() && _pRosBridge->ping() ) {
+            if ( !_pRosBridge->isRunning() && _pRosBridge->connected() ) {
                 _setupTopicService();
-            }
-
-            if ( !_pRosBridge->ping() ){
+            } else if ( _pRosBridge->isRunning() && !_pRosBridge->connected() ){
                 _pRosBridge->reset();
             }
-
-        } else {
-            if ( _pRosBridge->isRunning() )
+        } else  if ( _pRosBridge->isRunning() ) {
                 _pRosBridge->reset();
         }
     }

src/comm/ros_bridge/include/ROSBridge.h

@@ -40,7 +40,7 @@ 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 ping();
+    bool connected();
 
     bool isRunning();
 

src/comm/ros_bridge/include/RosBridgeClient.cpp

@@ -8,7 +8,7 @@
 void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shared_ptr<WsClient> client, const std::__cxx11::string &message)
 {
 #ifndef DEBUG
-    //(void)client_name;
+    (void)client_name;
 #endif
     if (!client->on_open)
     {
@@ -73,13 +73,52 @@ void RosbridgeWsClient::start(const std::__cxx11::string &client_name, std::shar
     client_thread.detach();
 }
 
-RosbridgeWsClient::RosbridgeWsClient(const std::__cxx11::string &server_port_path)
+RosbridgeWsClient::RosbridgeWsClient(const std::__cxx11::string &server_port_path) :
+    server_port_path(server_port_path)
+  , stopped(std::make_shared<std::atomic_bool>(false))
+  , hasConnection(std::make_shared<std::atomic_bool>(false))
 {
-    this->server_port_path = server_port_path;
+    // Start thread to monitor connection status.
+    // There might be a better way to do this.
+    std::thread t([this]  () {
+        const auto t_max = std::chrono::milliseconds(500);
+        while ( !this->stopped->load() ){
+            auto start = std::chrono::high_resolution_clock::now();
+            auto pPromise = std::make_shared<std::promise<void>>();
+            auto future = pPromise->get_future();
+
+            auto status_client = std::make_shared<WsClient>(this->server_port_path);
+            status_client->on_open = [pPromise](std::shared_ptr<WsClient::Connection>) {
+                    pPromise->set_value();
+            };
+
+            std::thread poll_thread([status_client]{
+                status_client->start();
+                status_client->on_open = NULL;
+                status_client->on_message = NULL;
+                status_client->on_close = NULL;
+                status_client->on_error = NULL;
+            });
+
+
+            auto status = future.wait_for(std::chrono::milliseconds(t_max));
+            status_client->stop();
+            poll_thread.join();
+            this->hasConnection->store(status == std::future_status::ready);
+            auto end = std::chrono::high_resolution_clock::now();
+            auto t_sleep = std::chrono::milliseconds(t_max)-(end-start);
+            if ( t_sleep > std::chrono::milliseconds(0)){
+                std::this_thread::sleep_for(t_sleep);
+            }
+        }
+        std::cout << "connection monitor thread end" << std::endl;
+    });
+    t.detach();
 }
 
 RosbridgeWsClient::~RosbridgeWsClient()
 {
+    stopped->store(true);
     unsubscribeAll();
     unadvertiseAll();
     unadvertiseAllServices();
@@ -91,29 +130,8 @@ RosbridgeWsClient::~RosbridgeWsClient()
 
 bool RosbridgeWsClient::connected(){
 
-        auto p = std::make_shared<std::promise<void>>();
-        auto future = p->get_future();
-
-        auto status_client = std::make_shared<WsClient>(server_port_path);
-        status_client->on_open = [p](std::shared_ptr<WsClient::Connection>) {
-                p->set_value();
-        };
-
-        std::thread t([status_client]{
-            status_client->start();
-            status_client->on_open = NULL;
-            status_client->on_message = NULL;
-            status_client->on_close = NULL;
-            status_client->on_error = NULL;
-        });
-
-        auto status = future.wait_for(std::chrono::milliseconds(20));
-        status_client->stop();
-        t.join();
-        bool connected = status == std::future_status::ready;
-        //std::cout << "connected(): " << connected << std::endl;
-        return connected;
-    return true;
+    std::cout << "connected " << hasConnection->load() << std::endl;
+    return hasConnection->load();
 }
 
 void RosbridgeWsClient::addClient(const std::string &client_name)
@@ -672,8 +690,7 @@ bool RosbridgeWsClient::serviceAvailable(const std::string &service)
 
 void RosbridgeWsClient::waitForService(const std::string &service)
 {
-    auto stop = std::make_shared<std::atomic_bool>(false);
-    waitForService(service, stop);
+    waitForService(service, stopped);
 }
 
 void RosbridgeWsClient::waitForService(const std::string &service, const std::shared_ptr<std::atomic_bool> stop)

src/comm/ros_bridge/include/RosBridgeClient.h

@@ -45,11 +45,13 @@ class RosbridgeWsClient
         WPClient = 3
     };
 
-  std::string server_port_path;
+  const std::string server_port_path;
   std::unordered_map<std::string /*client name*/,
                      std::shared_ptr<WsClient> /*client*/> client_map;
   std::deque<EntryData> service_topic_list;
   std::mutex mutex;
+  std::shared_ptr<std::atomic_bool> hasConnection;
+  std::shared_ptr<std::atomic_bool> stopped;
 
   void start(const std::string& client_name, std::shared_ptr<WsClient> client, const std::string& message);
 
@@ -58,7 +60,6 @@ public:
 
   ~RosbridgeWsClient();
 
- // The execution can take up to 100 ms!
  bool connected();
 
   // Adds a client to the client_map.

src/comm/ros_bridge/src/ROSBridge.cpp

@@ -44,24 +44,16 @@ 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()
+bool ROSBridge::ROSBridge::connected()
 {
     return _rbc.connected();
 }