rosbridge works good now

src/comm/ros_bridge/include/ROSBridge.h

@@ -45,7 +45,7 @@ public:
     bool isRunning();
 
 private:
-    bool                    _running;
+    std::shared_ptr<std::atomic_bool> _stopped;
     TypeFactory             _typeFactory;
     JsonFactory             _jsonFactory;
     CasePacker              _casePacker;

src/comm/ros_bridge/include/Server.cpp

@@ -5,6 +5,7 @@
 ROSBridge::ComPrivate::Server::Server(CasePacker &casePacker, RosbridgeWsClient &rbc) :
     _rbc(rbc)
   , _casePacker(casePacker)
+  , _stopped(std::make_shared<std::atomic_bool>(true))
 {
 
 }
@@ -14,10 +15,10 @@ void ROSBridge::ComPrivate::Server::advertiseService(const std::string &service,
                                                      const Server::CallbackType &userCallback)
 {
     std::string clientName = _serviceAdvertiserKey + service;
-    auto it = std::find(_clientList.begin(), _clientList.end(), clientName);
-    if (it != _clientList.end())
+    auto it = _serviceMap.find(clientName);
+    if (it != _serviceMap.end())
         return; // Service allready advertised.
-    _clientList.push_back(clientName);
+    _serviceMap.insert(std::make_pair(clientName, service));
     _rbc.addClient(clientName);
 
     auto rbc = &_rbc;
@@ -80,14 +81,16 @@ ROSBridge::ComPrivate::Server::~Server()
 
 void ROSBridge::ComPrivate::Server::start()
 {
-    _running = true;
+    _stopped->store(false);
 }
 
 void ROSBridge::ComPrivate::Server::reset()
 {
-    if (!_running)
+    if ( _stopped->load() )
         return;
-    for (const auto& str : _clientList)
-        _rbc.removeClient(str);
-    _clientList.clear();
+    for (const auto& item : _serviceMap){
+        _rbc.unadvertiseService(item.second);
+        _rbc.removeClient(item.first);
+    }
+    _serviceMap.clear();
 }

src/comm/ros_bridge/include/Server.h

@@ -5,12 +5,14 @@
 #include "ros_bridge/include/TypeFactory.h"
 #include "ros_bridge/include/CasePacker.h"
 
+#include <unordered_map>
+
 namespace ROSBridge {
 namespace ComPrivate {
 
 class Server
 {
-    typedef std::vector<std::string> ClientList;
+    typedef std::unordered_map<std::string, std::string>  ServiceMap;
 public:
     typedef std::function<JsonDocUPtr(JsonDocUPtr)> CallbackType;
 
@@ -33,8 +35,8 @@ private:
 
     RosbridgeWsClient  &_rbc;
     CasePacker         &_casePacker;
-    ClientList          _clientList;
-    bool                _running;
+    ServiceMap         _serviceMap;
+    std::shared_ptr<std::atomic_bool> _stopped;
 };
 } // namespace ComPrivate
 } // namespace ROSBridge

src/comm/ros_bridge/include/TopicPublisher.cpp

 #include "TopicPublisher.h"
 
-
-
-
-
-struct ROSBridge::ComPrivate::ThreadData
-{
-    const ROSBridge::CasePacker      &casePacker;
-    RosbridgeWsClient                &rbc;
-    ROSBridge::ComPrivate::JsonQueue &queue;
-    std::mutex                       &queueMutex;
-    const std::atomic<bool>          &running;
-    std::condition_variable          &cv;
-};
+#include <unordered_map>
 
 ROSBridge::ComPrivate::TopicPublisher::TopicPublisher(CasePacker &casePacker,
                                                       RosbridgeWsClient &rbc) :
-      _running(false)
+      _stopped(std::make_shared<std::atomic_bool>(true))
     , _casePacker(casePacker)
     , _rbc(rbc)
 {
@@ -30,25 +18,65 @@ ROSBridge::ComPrivate::TopicPublisher::~TopicPublisher()
 
 void ROSBridge::ComPrivate::TopicPublisher::start()
 {
-    if ( _running.load() )  // start called while thread running.
+    if ( !_stopped->load() )  // start called while thread running.
         return;
-    _running.store(true);
-    ROSBridge::ComPrivate::ThreadData data{
-                _casePacker,
-                _rbc,
-                _queue,
-                _queueMutex,
-                _running,
-                _cv
-    };
-    _pThread = std::make_unique<std::thread>(&ROSBridge::ComPrivate::transmittLoop, data);
+    _stopped->store(false);
+    _pThread = std::make_unique<std::thread>([this]{
+        // Init.
+        std::unordered_map<std::string, std::string> topicMap;
+        // Main Loop.
+        while( !this->_stopped->load()){
+            std::unique_lock<std::mutex> lk(this->_mutex);
+            // Check if new data available, wait if not.
+            if (this->_queue.empty()){
+                this->_cv.wait(lk); // Wait for condition, spurious wakeups don't matter in this case.
+                continue;
+            }
+            // Pop message from queue.
+            JsonDocUPtr pJsonDoc(std::move(this->_queue.front()));
+            this->_queue.pop_front();
+            lk.unlock();
+
+            // Get tag from Json message and remove it.
+            Tag tag;
+            bool ret = this->_casePacker.getTag(pJsonDoc, tag);
+            assert(ret); // Json message does not contain a tag;
+            (void)ret;
+            this->_casePacker.removeTag(pJsonDoc);
+
+            // Check if topic must be advertised.
+            // Advertised topics are stored in advertisedTopicsHashList as
+            // a hash.
+            std::string clientName = ROSBridge::ComPrivate::_topicAdvertiserKey
+                                   + tag.topic();
+            auto it =  topicMap.find(clientName);
+            if ( it ==  topicMap.end()) { // Need to advertise topic?
+                 topicMap.insert(std::make_pair(clientName, tag.topic()));
+                 this->_rbc.addClient(clientName);
+                 this->_rbc.advertise(clientName,
+                                   tag.topic(),
+                                   tag.messageType() );
+                 this->_rbc.waitForTopic(tag.topic(), this->_stopped); // Wait until topic is advertised.
+            }
+
+            // Publish Json message.
+            if ( !this->_stopped->load() )
+                this->_rbc.publish(tag.topic(), *pJsonDoc.get());
+        } // while loop
+
+        // Tidy up.
+        for (auto& it :  topicMap){
+            this->_rbc.unadvertise(it.second);
+            this->_rbc.removeClient(it.first);
+        }
+    });
 }
 
 void ROSBridge::ComPrivate::TopicPublisher::reset()
 {
-    if ( !_running.load() )  // stop called while thread not running.
+    if ( _stopped->load() )  // stop called while thread not running.
         return;
-    _running.store(false);
+    _stopped->store(true);
     _cv.notify_one(); // Wake publisher thread.
     if ( !_pThread )
         return;
@@ -56,51 +84,3 @@ void ROSBridge::ComPrivate::TopicPublisher::reset()
     _queue.clear();
 }
 
-void ROSBridge::ComPrivate::transmittLoop(ROSBridge::ComPrivate::ThreadData data)
-{
-    // Init.
-    ClientMap           clientMap;
-    // Main Loop.
-    while(data.running.load()){
-        std::unique_lock<std::mutex> lk(data.queueMutex);
-        // Check if new data available, wait if not.
-        if (data.queue.empty()){
-            data.cv.wait(lk); // Wait for condition, spurious wakeups don't matter in this case.
-            continue;
-        }
-        // Pop message from queue.
-        JsonDocUPtr pJsonDoc(std::move(data.queue.front()));
-        data.queue.pop_front();
-        lk.unlock();
-
-        // Get tag from Json message and remove it.
-        Tag tag;
-        bool ret = data.casePacker.getTag(pJsonDoc, tag);
-        assert(ret); // Json message does not contain a tag;
-        (void)ret;
-        data.casePacker.removeTag(pJsonDoc);
-
-        // Check if topic must be advertised.
-        // Advertised topics are stored in advertisedTopicsHashList as
-        // a hash.
-        std::string clientName = ROSBridge::ComPrivate::_topicAdvertiserKey
-                               + tag.topic();
-        HashType hash = ROSBridge::ComPrivate::getHash(clientName);
-        auto it = clientMap.find(hash);
-        if ( it == clientMap.end()) { // Need to advertise topic?
-            clientMap.insert(std::make_pair(hash, clientName));
-            data.rbc.addClient(clientName);
-            data.rbc.advertise(clientName,
-                               tag.topic(),
-                               tag.messageType() );
-        }
-
-        // Publish Json message.
-        data.rbc.publish(tag.topic(), *pJsonDoc.get());
-    } // while loop
-
-    // Tidy up.
-    for (auto& it : clientMap)
-        data.rbc.removeClient(it.second);
-}
-

src/comm/ros_bridge/include/TopicPublisher.h

@@ -37,7 +37,7 @@ public:
 
     void publish(JsonDocUPtr docPtr){
         {
-        std::lock_guard<std::mutex> lock(_queueMutex);
+        std::lock_guard<std::mutex> lock(_mutex);
         _queue.push_back(std::move(docPtr));
         }
         _cv.notify_one();  // Wake publisher thread.
@@ -51,8 +51,8 @@ public:
 
 private:
     JsonQueue           _queue;
-    std::mutex          _queueMutex;
-    std::atomic<bool>   _running;
+    std::mutex          _mutex;
+    std::shared_ptr<std::atomic_bool>   _stopped;
     CasePacker         &_casePacker;
     RosbridgeWsClient  &_rbc;
     CondVar             _cv;
@@ -60,8 +60,5 @@ private:
 };
 
 
-void transmittLoop(ThreadData data);
-
-
 } // namespace CommunicatorDetail
 } // namespace ROSBridge

src/comm/ros_bridge/include/TopicSubscriber.cpp

@@ -5,7 +5,7 @@ ROSBridge::ComPrivate::TopicSubscriber::TopicSubscriber(CasePacker &casePacker,
         RosbridgeWsClient &rbc) :
     _casePacker(casePacker)
   , _rbc(rbc)
-  , _running(false)
+  , _stopped(std::make_shared<std::atomic_bool>(true))
 {
 
 }
@@ -17,120 +17,78 @@ ROSBridge::ComPrivate::TopicSubscriber::~TopicSubscriber()
 
 void ROSBridge::ComPrivate::TopicSubscriber::start()
 {
-    _running = true;
+    _stopped->store(false);
 }
 
 void ROSBridge::ComPrivate::TopicSubscriber::reset()
 {
-    if ( _running ){
-        _running = false;
+    if ( !_stopped->load() ){
+        _stopped->store(true);
         {
-            for (std::string clientName : _clientList){
-                _rbc.removeClient(clientName);
+            for (auto &item : _topicMap){
+                _rbc.unsubscribe(item.second);
+                _rbc.removeClient(item.first);
             }
         }
-        {
-            std::lock_guard<std::mutex> lk(_callbackMapStruct.mutex);
-            _callbackMapStruct.map.clear();
-        }
-        _clientList.clear();
+        _topicMap.clear();
     }
 }
 
-bool ROSBridge::ComPrivate::TopicSubscriber::subscribe(
+void ROSBridge::ComPrivate::TopicSubscriber::subscribe(
         const char *topic,
         const std::function<void(JsonDocUPtr)> &callback)
 {
-    if ( !_running )
-        return false;
+    if ( _stopped->load() )
+        return;
 
     std::string clientName = ROSBridge::ComPrivate::_topicSubscriberKey
                            + std::string(topic);
-    _clientList.push_back(clientName);
-
-    HashType hash = getHash(clientName);
-    {
-        std::lock_guard<std::mutex> lk(_callbackMapStruct.mutex);
-        auto ret = _callbackMapStruct.map.insert(std::make_pair(hash, callback)); //
-        if ( !ret.second )
-            return false; // Topic subscription already present.
-
+    auto it = _topicMap.find(clientName);
+    if ( it != _topicMap.end() ){ // Topic already subscribed?
+        return;
     }
+    _topicMap.insert(std::make_pair(clientName, std::string(topic)));
 
     using namespace std::placeholders;
-    auto f = std::bind(&ROSBridge::ComPrivate::subscriberCallback,
-                      hash,
-                      std::ref(_callbackMapStruct),
-                      _1, _2);
-
-//    std::cout << std::endl;
-//    std::cout << "topic subscription" << std::endl;
-//    std::cout << "client name: " << clientName << std::endl;
-//    std::cout << "topic: " << topic << std::endl;
-    {
-        auto start = std::chrono::high_resolution_clock::now();
+    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;
+                }
+
+                // Extract message and call callback.
+                JsonDocUPtr pDoc(new JsonDoc());
+                pDoc->CopyFrom(docFull["msg"].Move(), docFull.GetAllocator());
+                callback(std::move(pDoc));
+                return;
+    };
+
+    if ( !_rbc.topicAvailable(topic) ){
+        // Wait until topic is available.
+        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);
+            }
+        });
+        t.detach();
+    } else {
         _rbc.addClient(clientName);
-        auto end = std::chrono::high_resolution_clock::now();
-        std::cout  << "add client time: "
-                   << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
-                   << " ms" << std::endl;
-        start = std::chrono::high_resolution_clock::now();
-        _rbc.subscribe(clientName,
-                        topic,
-                        f );
-        end = std::chrono::high_resolution_clock::now();
-        std::cout  << "subscribe time: "
-                   << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
-                   << " ms" << std::endl;
+        _rbc.subscribe(clientName, topic, callbackWrapper);
     }
-
-    return true;
 }
 
 
-using WsClient = SimpleWeb::SocketClient<SimpleWeb::WS>;
-void ROSBridge::ComPrivate::subscriberCallback(
-        const HashType &hash,
-        ROSBridge::ComPrivate::MapStruct &mapWrapper,
-        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::cout << "Json document: "
-//              << in_message->string()
-//              << std::endl;
-
-    // Search callback.
-    CallbackType callback;
-    {
-        std::lock_guard<std::mutex> lk(mapWrapper.mutex);
-        auto it = mapWrapper.map.find(hash);
-        if (it == mapWrapper.map.end()) {
-            //assert(false); // callback not found
-            return;
-        }
-        callback = it->second;
-    }
-
-    // Extract message and call callback.
-    JsonDocUPtr pDoc(new JsonDoc());
-    pDoc->CopyFrom(docFull["msg"].Move(), docFull.GetAllocator());
-    callback(std::move(pDoc));
-    return;
-
-}

src/comm/ros_bridge/include/TopicSubscriber.h

@@ -5,20 +5,17 @@
 #include "ros_bridge/include/TypeFactory.h"
 #include "ros_bridge/include/CasePacker.h"
 
+#include <unordered_map>
+
 namespace ROSBridge {
 namespace ComPrivate {
 
 
 typedef std::function<void(JsonDocUPtr)> CallbackType;
-struct MapStruct{
-    typedef std::map<HashType, CallbackType> Map;
-    Map map;
-    std::mutex mutex;
-};
 
 class TopicSubscriber
 {
-    typedef std::vector<std::string> ClientList;
+    typedef std::unordered_map<std::string, std::string> TopicMap;
 public:
 
     TopicSubscriber() = delete;
@@ -34,7 +31,7 @@ public:
     //! @return Returns false if a subscription to this topic allready exists.
     //!
     //! @note Only one callback can be registered.
-    bool subscribe(const char* topic, const CallbackType &callback);
+    void subscribe(const char* topic, const CallbackType &callback);
 
 private:
 
@@ -42,14 +39,9 @@ private:
 
     CasePacker         &_casePacker;
     RosbridgeWsClient  &_rbc;
-    MapStruct           _callbackMapStruct;
-    ClientList          _clientList;
-    bool                _running;
+    TopicMap            _topicMap;
+    std::shared_ptr<std::atomic_bool>   _stopped;
 };
 
-void subscriberCallback(const HashType &hash,
-                        MapStruct &mapWrapper,
-                        std::shared_ptr<WsClient::Connection> /*connection*/,
-                        std::shared_ptr<WsClient::InMessage> in_message);
 } // namespace ComPrivate
 } // namespace ROSBridge

src/comm/ros_bridge/src/ROSBridge.cpp

 #include "ros_bridge/include/ROSBridge.h"
 
 ROSBridge::ROSBridge::ROSBridge() :
-    _running(false)
+    _stopped(std::make_shared<std::atomic_bool>(true))
   ,  _casePacker(&_typeFactory, &_jsonFactory)
   , _rbc("localhost:9090")
   , _topicPublisher(_casePacker, _rbc)
@@ -38,7 +38,7 @@ void ROSBridge::ROSBridge::start()
     _topicPublisher.start();
     _topicSubscriber.start();
     _server.start();
-    _running = true;
+    _stopped->store(false);
 }
 
 void ROSBridge::ROSBridge::reset()
@@ -46,7 +46,7 @@ void ROSBridge::ROSBridge::reset()
     _topicPublisher.reset();
     _topicSubscriber.reset();
     _server.reset();
-    _running = false;
+    _stopped->store(true);
 }
 
 bool ROSBridge::ROSBridge::ping()
@@ -56,6 +56,6 @@ bool ROSBridge::ROSBridge::ping()
 
 bool ROSBridge::ROSBridge::isRunning()
 {
-    return _running;
+    return !_stopped->load();
 }