123

src/Snake/snake.cpp

@@ -4,6 +4,13 @@
 #include "clipper/clipper.hpp"
 #define CLIPPER_SCALE 10000
 
+#include "ortools/constraint_solver/routing.h"
+#include "ortools/constraint_solver/routing_enums.pb.h"
+#include "ortools/constraint_solver/routing_index_manager.h"
+#include "ortools/constraint_solver/routing_parameters.h"
+
+using namespace operations_research;
+
 #ifndef NDEBUG
 //#define SHOW_TIME
 #endif
@@ -37,7 +44,7 @@ bool Scenario::addArea(Area &area)
         return false;
 }
 
-bool Scenario::defined(double tileWidth, double tileHeight, double minTileArea)
+bool Scenario::update(double tileWidth, double tileHeight, double minTileArea)
 {
     if (!_areas2enu())
         return false;
@@ -301,6 +308,14 @@ bool Scenario::_calculateJoinedArea()
     return true;
 }
 
+
+
+struct FlightPlan::RoutingDataModel{
+    Matrix<int64_t>                 distanceMatrix;
+    long                            numVehicles;
+    RoutingIndexManager::NodeIndex  depot;
+};
+
 FlightPlan::FlightPlan()
 {
 
@@ -356,7 +371,7 @@ bool FlightPlan::generate(double lineDistance, double minTransectLength)
     }
     size_t n1 = vertices.size();
     // Generate routing model.
-    RoutingDataModel_t dataModel;
+    RoutingDataModel dataModel;
     Matrix<double> connectionGraph(n1, n1);
 
 #ifdef SHOW_TIME
@@ -612,7 +627,7 @@ bool FlightPlan::_generateTransects(double lineDistance, double minTransectLengt
 void FlightPlan::_generateRoutingModel(const BoostLineString &vertices,
                                        const BoostPolygon &polygonOffset,
                                        size_t n0,
-                                       FlightPlan::RoutingDataModel_t &dataModel,
+                                       FlightPlan::RoutingDataModel &dataModel,
                                        Matrix<double> &graph)
 {
 #ifdef SHOW_TIME

src/Snake/snake.h

@@ -6,16 +6,9 @@
 
 #include "snake_geometry.h"
 
-#include "ortools/constraint_solver/routing.h"
-#include "ortools/constraint_solver/routing_enums.pb.h"
-#include "ortools/constraint_solver/routing_index_manager.h"
-#include "ortools/constraint_solver/routing_parameters.h"
-
 using namespace std;
 using namespace snake_geometry;
 
-using namespace operations_research;
-
 namespace snake {
 
     enum AreaType {MeasurementArea, ServiceArea, Corridor};
@@ -60,7 +53,7 @@ namespace snake {
         const vector<GeoPoint3D>        &getTileCenterPoints()    {return _tileCenterPoints;}
         const GeoPoint3D                &getHomePositon()         {return _homePosition;}
 
-        bool defined(double tileWidth, double tileHeight, double minTileArea);
+        bool update(double tileWidth, double tileHeight, double minTileArea);
 
         string errorString;
 
@@ -104,8 +97,8 @@ namespace snake {
     public:
         FlightPlan();
 
-        void setScenario(const Scenario &scenario)          {_scenario = scenario;}
-        void setProgress(const vector<int8_t> &progress)    {_progress = progress;}
+        void setScenario(Scenario &scenario)          {_scenario = scenario;}
+        void setProgress(vector<int> &progress)       {_progress = progress;}
 
         const Scenario          &getScenario(void)          const {return _scenario;}
         const BoostLineString   &getWaypointsENU(void)      const {return _waypointsENU;}
@@ -124,28 +117,22 @@ namespace snake {
     private:
         // Search Filter to speed up routing.SolveWithParameters(...);
         // Found here: http://www.lia.disi.unibo.it/Staff/MicheleLombardi/or-tools-doc/user_manual/manual/ls/ls_filtering.html
-        class SearchFilter : public LocalSearchFilter{
-
-        };
+        class SearchFilter;
 
-        typedef struct{
-            Matrix<int64_t>                 distanceMatrix;
-            long                            numVehicles;
-            RoutingIndexManager::NodeIndex  depot;
-        }RoutingDataModel_t;
+        struct RoutingDataModel;
 
         bool _generateTransects(double lineDistance, double minTransectLength);
         void _generateRoutingModel(const BoostLineString &vertices,
                                    const BoostPolygon &polygonOffset,
                                    size_t n0,
-                                   RoutingDataModel_t &dataModel,
+                                   RoutingDataModel &dataModel,
                                    Matrix<double> &graph);
 
         Scenario                                _scenario;
         BoostLineString                         _waypointsENU;
         GeoPoint2DList                          _waypoints;
         vector<BoostLineString>                 _transects;
-        vector<int8_t>                          _progress;
+        vector<int>                             _progress;
         vector<uint64_t>                        _arrivalPathIdx;
         vector<uint64_t>                        _returnPathIdx;
 

src/Wima/Geometry/GeoPoint3D.cpp

 #include "GeoPoint3D.h"
 
-GeoPoint3D::GeoPoint3D(QObject *parent)
+GeoPoint::GeoPoint(QObject *parent)
     : QObject(parent), ROSGeoPoint() {}
 
-GeoPoint3D::GeoPoint3D(double latitude, double longitude, double altitude, QObject *parent)
+GeoPoint::GeoPoint(double latitude, double longitude, double altitude, QObject *parent)
     : QObject(parent), ROSGeoPoint(latitude, longitude, altitude)
 {}
 
-GeoPoint3D::GeoPoint3D(const GeoPoint3D &p, QObject *parent)
+GeoPoint::GeoPoint(const GeoPoint &p, QObject *parent)
     : QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
 {}
 
-GeoPoint3D::GeoPoint3D(const ROSGeoPoint &p, QObject *parent)
+GeoPoint::GeoPoint(const ROSGeoPoint &p, QObject *parent)
     : QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
 {}
 
-GeoPoint3D::GeoPoint3D(const QGeoCoordinate &p, QObject *parent)
+GeoPoint::GeoPoint(const QGeoCoordinate &p, QObject *parent)
     : QObject(parent), ROSGeoPoint(p.latitude(), p.longitude(), p.altitude())
 {}
 
-GeoPoint3D *GeoPoint3D::Clone() const
+GeoPoint *GeoPoint::Clone() const
 {
-    return new GeoPoint3D(*this);
+    return new GeoPoint(*this);
 }
 
-GeoPoint3D &GeoPoint3D::operator=(const GeoPoint3D &p)
+GeoPoint &GeoPoint::operator=(const GeoPoint &p)
 {
     this->setLatitude(p.latitude());
     this->setLongitude(p.longitude());
@@ -32,7 +32,7 @@ GeoPoint3D &GeoPoint3D::operator=(const GeoPoint3D &p)
 
     return *this;
 }
-GeoPoint3D &GeoPoint3D::operator=(const QGeoCoordinate &p)
+GeoPoint &GeoPoint::operator=(const QGeoCoordinate &p)
 {
     this->setLatitude(p.latitude());
     this->setLongitude(p.longitude());

src/Wima/Geometry/GeoPoint3D.h

@@ -11,27 +11,27 @@
 typedef ROSBridge::MessageBaseClass ROSMsg;
 typedef ROSBridge::GenericMessages::GeographicMsgs::GeoPoint ROSGeoPoint;
 namespace MsgGroups = ROSBridge::MessageGroups;
-class GeoPoint3D : public QObject, public ROSGeoPoint
+class GeoPoint : public QObject, public ROSGeoPoint
 {
     Q_OBJECT
 public:
     typedef MsgGroups::GeoPointGroup Group;
 
-    GeoPoint3D(QObject *parent = nullptr);
-    GeoPoint3D(double latitude,
+    GeoPoint(QObject *parent = nullptr);
+    GeoPoint(double latitude,
                double longitude,
                double altitude,
                QObject *parent = nullptr);
-    GeoPoint3D(const GeoPoint3D& p,
+    GeoPoint(const GeoPoint& p,
                QObject *parent = nullptr);
-    GeoPoint3D(const ROSGeoPoint& p,
+    GeoPoint(const ROSGeoPoint& p,
                QObject *parent = nullptr);
-    GeoPoint3D(const QGeoCoordinate& p,
+    GeoPoint(const QGeoCoordinate& p,
                QObject *parent = nullptr);
 
-    virtual GeoPoint3D *Clone() const override;
-    GeoPoint3D &operator=(const GeoPoint3D&p);
-    GeoPoint3D &operator=(const QGeoCoordinate&p);
+    virtual GeoPoint *Clone() const override;
+    GeoPoint &operator=(const GeoPoint&p);
+    GeoPoint &operator=(const QGeoCoordinate&p);
 
 };
 

src/Wima/Snake/SnakeWorker.cc

@@ -2,58 +2,84 @@
 #include <QGeoCoordinate>
 
 
+#include "snake_geometry.h"
+#include "snake.h"
+
+
+
+using namespace snake;
+using namespace snake_geometry;
+
 SnakeWorker::SnakeWorker(QObject *parent) : QThread(parent)
-  , _lineDistance       (3) // meter
-  , _minTransectLength  (2) // meter
+    , _lineDistance(3)
+    , _minTransectLength(2)
+    , _tileWidth(5)
+    , _tileHeight(5)
+    , _minTileArea(3)
 {
 }
 
 SnakeWorker::~SnakeWorker()
 {
-}
 
-void SnakeWorker::setScenario(const Scenario &scenario)
-{
-    _scenario = scenario;
 }
 
-void SnakeWorker::setProgress(const QVector<int> &progress)
+WorkerResult_t &SnakeWorker::result()
 {
-    _progress.clear();
-    for (auto p : progress) {
-        assert(p >= -1 && p <= 100);
-        _progress.push_back(p);
-    }
+    return _result;
 }
 
-void SnakeWorker::setLineDistance(double lineDistance)
+bool SnakeWorker::precondition() const
 {
-    assert(lineDistance > 0);
-    _lineDistance = lineDistance;
-}
 
-void SnakeWorker::setMinTransectLength(double minTransectLength)
-{
-    assert(minTransectLength > 0);
-    _minTransectLength = minTransectLength;
+    return _mArea.size() > 0
+            && _sArea.size() > 0
+            && _corridor.size() > 0
+            && _progress.size() > 0;
 }
 
-const WorkerResult_t &SnakeWorker::getResult() const
+double SnakeWorker::lineDistance() const
 {
-    return _result;
+    return _lineDistance;
 }
 
 void SnakeWorker::run()
 {
     // Reset _result struct.
-    _result.success = false;
-    _result.errorMessage = "";
-    _result.waypoints.clear();
-    _result.returnPathIdx.clear();
-    _result.arrivalPathIdx.clear();
+    _result.clear();
+
+    if ( !precondition())
+        return;
+
+    Scenario scenario;// Initialize scenario.
+
+    Area mArea;
+    for (auto vertex : _mArea){
+        mArea.geoPolygon.push_back(GeoPoint2D{vertex.latitude(), vertex.longitude()});
+    }
+    mArea.type = AreaType::MeasurementArea;
+
+    Area sArea;
+    for (auto vertex : _sArea){
+        sArea.geoPolygon.push_back(GeoPoint2D{vertex.latitude(), vertex.longitude()});
+    }
+    sArea.type = AreaType::ServiceArea;
+
+    Area corridor;
+    for (auto vertex : _corridor){
+        corridor.geoPolygon.push_back(GeoPoint2D{vertex.latitude(), vertex.longitude()});
+    }
+    corridor.type = AreaType::Corridor;
+
+    scenario.addArea(mArea);
+    scenario.addArea(sArea);
+    scenario.addArea(corridor);
+
+    if ( !scenario.update(_tileWidth, _tileHeight, _minTileArea) )
+        return;
 
     FlightPlan flightplan;
-    flightplan.setScenario(_scenario);
+    flightplan.setScenario(scenario);
     flightplan.setProgress(_progress);
 
     // Trying to generate flight plan.
@@ -67,20 +93,116 @@ void SnakeWorker::run()
         _result.success = true;
 
         // Fill result struct.
-        auto waypoints = flightplan.getWaypoints();
+        auto& waypoints = flightplan.getWaypoints();
         for (auto vertex : waypoints){
-            _result.waypoints.append(QVariant::fromValue(QGeoCoordinate(vertex[0], vertex[1], 0)));
+            _result.waypoints.append(QGeoCoordinate(vertex[0], vertex[1], 0));
         }
 
-        auto arrivalPathIdx = flightplan.getArrivalPathIdx();
+        auto& arrivalPathIdx = flightplan.getArrivalPathIdx();
         for (auto idx : arrivalPathIdx){
             _result.arrivalPathIdx.append(idx);
         }
 
-        auto returnPathIdx = flightplan.getReturnPathIdx();
+        auto& returnPathIdx = flightplan.getReturnPathIdx();
         for (auto idx : returnPathIdx){
             _result.returnPathIdx.append(idx);
         }
 
+
+        // Get tiles and origin.
+        auto origin = scenario.getOrigin();
+        _result.origin.setLatitude(origin[0]);
+        _result.origin.setLongitude(origin[1]);
+        _result.origin.setAltitude(origin[2]);
+        const auto &tiles = scenario.getTiles();
+        const auto &cps = scenario.getTileCenterPoints();
+        for ( unsigned int i=0; i < tiles.size(); ++i ) {
+            const auto &tile = tiles[i];
+            SnakeTile Tile;
+            for ( const auto &vertex : tile) {
+                QGeoCoordinate QVertex(vertex[0], vertex[1], vertex[2]);
+                Tile.append(QVertex);
+            }
+            const auto &centerPoint = cps[i];
+            QGeoCoordinate QCenterPoint(centerPoint[0], centerPoint[1], centerPoint[2]);
+            Tile.setCenter(QCenterPoint);
+            _result.tiles.polygons().append(Tile);
+            _result.tileCenterPoints.append(QVariant::fromValue(QCenterPoint));
+         }
+
+        // Get local tiles.
+        const auto &tilesENU = scenario.getTilesENU();
+        for ( unsigned int i=0; i < tilesENU.size(); ++i ) {
+            const auto &tile = tilesENU[i];
+            Polygon2D Tile;
+            for ( const auto &vertex : tile.outer()) {
+                QPointF QVertex(vertex.get<0>(), vertex.get<1>());
+                Tile.path().append(QVertex);
             }
+            _result.tilesLocal.polygons().append(Tile);
+        }
+
+    }
+}
+
+double SnakeWorker::minTransectLength() const
+{
+    return _minTransectLength;
+}
+
+void SnakeWorker::setMinTransectLength(double minTransectLength)
+{
+    if (minTransectLength > 0)
+        _minTransectLength = minTransectLength;
+}
+
+double SnakeWorker::minTileArea() const
+{
+    return _minTileArea;
+}
+
+void SnakeWorker::setLineDistance(double lineDistance)
+{
+    if (lineDistance > 0)
+        _lineDistance = lineDistance;
+}
+
+double SnakeWorker::tileWidth() const
+{
+    return _tileWidth;
+}
+
+void SnakeWorker::setTileWidth(double tileWidth)
+{
+    if (tileWidth > 0)
+        _tileWidth = tileWidth;
+}
+
+double SnakeWorker::tileHeight() const
+{
+    return _tileHeight;
+}
+
+void SnakeWorker::setTileHeight(double tileHeight)
+{
+    if (tileHeight > 0)
+        _tileHeight = tileHeight;
+}
+
+void SnakeWorker::setMinTileArea(double minTileArea)
+{
+    if (minTileArea > 0)
+        _minTileArea = minTileArea;
+}
+
+void Result::clear()
+{
+    this->success = false;
+    this->errorMessage = "";
+    this->waypoints.clear();
+    this->returnPathIdx.clear();
+    this->arrivalPathIdx.clear();
+    this->tiles.polygons().clear();
+    this->tilesLocal.polygons().clear();
+    this->tileCenterPoints.clear();
 }

src/Wima/Snake/SnakeWorker.h

@@ -4,23 +4,27 @@
 #include <QVariant>
 #include <QThread>
 #include <QVector>
+#include <QGeoCoordinate>
+#include <vector>
 
-#include "snake_geometry.h"
-#include "snake.h"
-
-using namespace snake;
-using namespace snake_geometry;
-using namespace std;
+#include "SnakeTiles.h"
+#include "SnakeTilesLocal.h"
 
 
 typedef QList<QVariant> QVariantList;
 
 typedef struct Result{
-    QVariantList            waypoints;
+    QVector<QGeoCoordinate> waypoints;
+    SnakeTiles              tiles;
+    SnakeTilesLocal         tilesLocal;
+    QVariantList            tileCenterPoints;
+    QGeoCoordinate          origin;
     QVector<unsigned long>  arrivalPathIdx;
     QVector<unsigned long>  returnPathIdx;
     bool                    success;
     QString                 errorMessage;    
+
+    void clear();
 }WorkerResult_t;
 
 class SnakeWorker : public QThread{
@@ -30,21 +34,86 @@ public:
     SnakeWorker(QObject *parent = nullptr);
     ~SnakeWorker() override;
 
-    void setScenario            (const Scenario  &scenario);
-    void setProgress            (const QVector<int> &progress);
-    void setLineDistance        (double lineDistance);
-    void setMinTransectLength   (double minTransectLength);
 
-    const WorkerResult_t &getResult() const;
+    template<template<class, class...> class Container>
+    void setMeasurementArea     (const Container<QGeoCoordinate>  &measurementArea);
+    template<template<class, class...> class Container>
+    void setServiceArea         (const Container<QGeoCoordinate>  &serviceArea);
+    template<template<class, class...> class Container>
+    void setCorridor            (const Container<QGeoCoordinate>  &corridor);
+    template<template<class, class...> class Container, class IntType>
+    void setProgress            (const Container<IntType>  &progress);
+
+    WorkerResult_t &result();
+
+    double lineDistance() const;
+    void setLineDistance(double lineDistance);
+
+    double minTransectLength() const;
+    void setMinTransectLength(double minTransectLength);
+
+    double minTileArea() const;
+    void setMinTileArea(double minTileArea);
+
+    double tileHeight() const;
+    void setTileHeight(double tileHeight);
+
+    double tileWidth() const;
+    void setTileWidth(double tileWidth);
 
 protected:
     void run() override;
 
 private:
-    Scenario        _scenario;
-    vector<int8_t>  _progress;
+    bool precondition() const;
+
+
+    std::vector<QGeoCoordinate> _mArea;
+    std::vector<QGeoCoordinate> _sArea;
+    std::vector<QGeoCoordinate> _corridor;
+    std::vector<int>            _progress;
     double                      _lineDistance;
     double                      _minTransectLength;
+    double                      _tileWidth;
+    double                      _tileHeight;
+    double                      _minTileArea;
     WorkerResult_t              _result;
 };
 
+template<template<class, class...> class Container>
+void SnakeWorker::setCorridor(const Container<QGeoCoordinate> &corridor)
+{
+    _corridor.clear();
+    for (auto vertex : corridor) {
+        _corridor.push_back(vertex);
+    }
+}
+
+template<template<class, class...> class Container>
+void SnakeWorker::setMeasurementArea(const Container<QGeoCoordinate> &measurementArea)
+{
+    _mArea.clear();
+    for (auto vertex : measurementArea) {
+        _mArea.push_back(vertex);
+    }
+}
+
+template<template<class, class...> class Container>
+void SnakeWorker::setServiceArea(const Container<QGeoCoordinate> &serviceArea)
+{
+    _sArea.clear();
+    for (auto vertex : serviceArea) {
+        _sArea.push_back(vertex);
+    }
+}
+
+template<template<class, class...> class Container, class IntType>
+void SnakeWorker::setProgress(const Container<IntType> &progress)
+{
+    _progress.clear();
+    for (auto p : progress) {
+        assert(p >= -1 && p <= 100);
+        _progress.push_back(p);
+    }
+}
+

src/Wima/WaypointManager/Slicer.cpp

@@ -43,14 +43,13 @@ void Slicer::_updateIdx(long size)
 {
     _overlap = _overlap < _N ? _overlap : _N-1;
 
-    long maxStart  = size-_N;
-    _idxStart = _idxStart <= maxStart   ? _idxStart : maxStart;
+    _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 = _idxEnd == size -1 ? _idxStart : _idxEnd + 1 - _overlap;
     _idxNext = _idxNext < 0       ? 0         : _idxNext;
     _idxNext = _idxNext < size    ? _idxNext  : size-1;
 

src/Wima/WimaController.h

@@ -338,10 +338,7 @@ private slots:
     void    _smartRTLCleanUp                    (bool flying);
     // Snake.
     void _setSnakeCalcInProgress             (bool inProgress);
-    bool _isScenarioDefined                  (void);
-    bool _isScenarioDefinedErrorMessage      (void);
     void _snakeStoreWorkerResults            ();
-    void _startStopRosBridge                 ();
     void _initStartSnakeWorker               ();
     void _switchSnakeManager                 (QVariant variant);
     // Periodic tasks.
@@ -357,7 +354,6 @@ private:
     MissionController      *_missionController;
 
     // Wima Data.
-
     QmlObjectListModel      _areas; // contains all visible areas
     WimaJoinedAreaData      _joinedArea; // joined area fromed by opArea, serArea, _corridor
     WimaMeasurementAreaData _measurementArea; // measurement area
@@ -372,6 +368,8 @@ private:
     WaypointManager::DefaultManager     _snakeManager;
     WaypointManager::RTLManager         _rtlManager;
     WaypointManager::ManagerBase       *_currentManager;
+    using ManagerList = QList<WaypointManager::ManagerBase*>;
+    ManagerList                         _managerList;
 
     // Settings Facts.
     QMap<QString, FactMetaData*> _metaDataMap;
@@ -403,11 +401,8 @@ private:
 
     // Snake
     bool                    _snakeCalcInProgress;
-    bool                    _snakeRecalcNecessary;
-    bool                    _scenarioDefinedBool;
     SnakeWorker             _snakeWorker;
-    Scenario                _scenario;
-    ::GeoPoint3D            _snakeOrigin;
+    GeoPoint              _snakeOrigin;
     SnakeTiles              _snakeTiles; // tiles
     SnakeTilesLocal         _snakeTilesLocal; // tiles local coordinate system
     QVariantList            _snakeTileCenterPoints;

src/comm/ros_bridge/include/ROSBridge.h

@@ -45,7 +45,6 @@ private:
     JsonFactory             _jsonFactory;
     CasePacker              _casePacker;
     RosbridgeWsClient       _rbc;
-    std::mutex              _rbcMutex;
     ComPrivate::TopicPublisher     _topicPublisher;
     ComPrivate::TopicSubscriber    _topicSubscriber;
 

src/comm/ros_bridge/include/RosBridgeClient.h

@@ -17,6 +17,7 @@
 #include <thread>
 #include <future>
 #include <functional>
+#include <mutex>
 
 using WsClient = SimpleWeb::SocketClient<SimpleWeb::WS>;
 using InMessage =  std::function<void(std::shared_ptr<WsClient::Connection>, std::shared_ptr<WsClient::InMessage>)>;
@@ -25,6 +26,7 @@ class RosbridgeWsClient
 {
  std::string server_port_path;
  std::unordered_map<std::string, std::shared_ptr<WsClient>> client_map;
+ std::mutex map_mutex;
 
  // Starts the client.
  void start(const std::string& client_name, std::shared_ptr<WsClient> client, const std::string& message)
@@ -86,9 +88,6 @@ class RosbridgeWsClient
    });
 
    client_thread.detach();
-
-   // This is to make sure that the thread got fully launched before we do anything to it (e.g. remove)
-   std::this_thread::sleep_for(std::chrono::milliseconds(100));
  }
 
 public:
@@ -99,6 +98,8 @@ public:
 
  ~RosbridgeWsClient()
  {
+
+   std::lock_guard<std::mutex> lk(map_mutex); // neccessary?
    for (auto& client : client_map)
    {
      client.second->stop();
@@ -136,6 +137,8 @@ public:
  // Adds a client to the client_map.
  void addClient(const std::string& client_name)
  {
+
+   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())
    {
@@ -151,6 +154,8 @@ public:
 
  std::shared_ptr<WsClient> getClient(const std::string& client_name)
  {
+
+   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())
    {
@@ -161,6 +166,8 @@ public:
 
  void stopClient(const std::string& client_name)
  {
+
+   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())
    {
@@ -183,12 +190,22 @@ public:
 
  void removeClient(const std::string& client_name)
  {
+
+   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())
    {
-     it->second->stop();
-     it->second.reset();
+     // Stop the client asynchronously in 100 ms.
+     // This is to ensure, that all threads involving 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();
 #ifdef ROS_BRIDGE_CLIENT_DEBUG
      std::cout << client_name << " has been removed" << std::endl;
 #endif
@@ -205,6 +222,8 @@ 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())
    {
@@ -258,6 +277,8 @@ public:
 
  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::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
    if (it != client_map.end())
    {
@@ -302,6 +323,8 @@ 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::unordered_map<std::string, std::shared_ptr<WsClient>>::iterator it = client_map.find(client_name);
    if (it != client_map.end())
    {

src/comm/ros_bridge/include/TopicPublisher.cpp

@@ -8,20 +8,17 @@ struct ROSBridge::ComPrivate::ThreadData
 {
     const ROSBridge::CasePacker      &casePacker;
     RosbridgeWsClient                &rbc;
-    std::mutex                       &rbcMutex;
     ROSBridge::ComPrivate::JsonQueue &queue;
     std::mutex                       &queueMutex;
     const std::atomic<bool>          &running;
     std::condition_variable          &cv;
 };
 
-ROSBridge::ComPrivate::TopicPublisher::TopicPublisher(CasePacker *casePacker,
-                                                      RosbridgeWsClient *rbc,
-                                                      std::mutex *rbcMutex) :
+ROSBridge::ComPrivate::TopicPublisher::TopicPublisher(CasePacker &casePacker,
+                                                      RosbridgeWsClient &rbc) :
       _running(false)
     , _casePacker(casePacker)
     , _rbc(rbc)
-    , _rbcMutex(rbcMutex)
 {
 
 }
@@ -37,9 +34,8 @@ void ROSBridge::ComPrivate::TopicPublisher::start()
         return;
     _running.store(true);
     ROSBridge::ComPrivate::ThreadData data{
-                *_casePacker,
-                *_rbc,
-                *_rbcMutex,
+                _casePacker,
+                _rbc,
                 _queue,
                 _queueMutex,
                 _running,
@@ -64,11 +60,6 @@ void ROSBridge::ComPrivate::transmittLoop(ROSBridge::ComPrivate::ThreadData data
 {
     // Init.
     ClientMap           clientMap;
-//    {
-//        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);
@@ -101,27 +92,18 @@ void ROSBridge::ComPrivate::transmittLoop(ROSBridge::ComPrivate::ThreadData data
             std::cout << clientName << ";"
                       << tag.topic() << ";"
                       << tag.messageType() << ";" << std::endl;
-            {
-                std::lock_guard<std::mutex> lk(data.rbcMutex);
             data.rbc.addClient(clientName);
             data.rbc.advertise(clientName,
                                tag.topic(),
                                tag.messageType() );
         }
-        }
 
         // 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);
     for (auto& it : clientMap)
         data.rbc.removeClient(it.second);
-    }
 }
 

src/comm/ros_bridge/include/TopicPublisher.h

@@ -24,9 +24,8 @@ class TopicPublisher
 public:
 
     TopicPublisher() = delete;
-    TopicPublisher(CasePacker *casePacker,
-                   RosbridgeWsClient *rbc,
-                   std::mutex *rbcMutex);
+    TopicPublisher(CasePacker &casePacker,
+                   RosbridgeWsClient &rbc);
 
     ~TopicPublisher();
 
@@ -46,7 +45,7 @@ public:
 
     template<class T>
     void publish(const T &msg, const std::string &topic){
-        JsonDocUPtr docPtr(_casePacker->pack(msg, topic));
+        JsonDocUPtr docPtr(_casePacker.pack(msg, topic));
         publish(std::move(docPtr));
     }
 
@@ -54,9 +53,8 @@ private:
     JsonQueue           _queue;
     std::mutex          _queueMutex;
     std::atomic<bool>   _running;
-    CasePacker         *_casePacker;
-    RosbridgeWsClient  *_rbc;
-    std::mutex         *_rbcMutex;
+    CasePacker         &_casePacker;
+    RosbridgeWsClient  &_rbc;
     CondVar             _cv;
     ThreadPtr           _pThread;
 };

src/comm/ros_bridge/include/TopicSubscriber.cpp

 #include "TopicSubscriber.h"
 
 
-ROSBridge::ComPrivate::TopicSubscriber::TopicSubscriber(ROSBridge::CasePacker *casePacker,
-        RosbridgeWsClient *rbc, std::mutex *rbcMutex) :
+ROSBridge::ComPrivate::TopicSubscriber::TopicSubscriber(CasePacker &casePacker,
+        RosbridgeWsClient &rbc) :
     _casePacker(casePacker)
   , _rbc(rbc)
-  , _rbcMutex(rbcMutex)
   , _running(false)
 {
 
@@ -26,14 +25,13 @@ void ROSBridge::ComPrivate::TopicSubscriber::reset()
     if ( _running ){
         _running = false;
         {
-            std::lock_guard<std::mutex> lk(*_rbcMutex);
             for (std::string clientName : _clientList){
-                _rbc->removeClient(clientName);
+                _rbc.removeClient(clientName);
             }
         }
         {
-            std::lock_guard<std::mutex> lk(_callbackMap.mutex);
-            _callbackMap.map.clear();
+            std::lock_guard<std::mutex> lk(_callbackMapStruct.mutex);
+            _callbackMapStruct.map.clear();
         }
         _clientList.clear();
     }
@@ -52,8 +50,8 @@ bool ROSBridge::ComPrivate::TopicSubscriber::subscribe(
 
     HashType hash = getHash(clientName);
     {
-        std::lock_guard<std::mutex> lk(_callbackMap.mutex);
-        auto ret = _callbackMap.map.insert(std::make_pair(hash, callback)); //
+        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.
 
@@ -62,7 +60,7 @@ bool ROSBridge::ComPrivate::TopicSubscriber::subscribe(
     using namespace std::placeholders;
     auto f = std::bind(&ROSBridge::ComPrivate::subscriberCallback,
                       hash,
-                      std::ref(_callbackMap),
+                      std::ref(_callbackMapStruct),
                       _1, _2);
 
 //    std::cout << std::endl;
@@ -70,11 +68,20 @@ bool ROSBridge::ComPrivate::TopicSubscriber::subscribe(
 //    std::cout << "client name: " << clientName << std::endl;
 //    std::cout << "topic: " << topic << std::endl;
     {
-        std::lock_guard<std::mutex> lk(*_rbcMutex);
-        _rbc->addClient(clientName);
-        _rbc->subscribe(clientName,
+        auto start = std::chrono::high_resolution_clock::now();
+        _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;
     }
 
     return true;
@@ -84,7 +91,7 @@ bool ROSBridge::ComPrivate::TopicSubscriber::subscribe(
 using WsClient = SimpleWeb::SocketClient<SimpleWeb::WS>;
 void ROSBridge::ComPrivate::subscriberCallback(
         const HashType &hash,
-        ROSBridge::ComPrivate::CallbackMapWrapper &mapWrapper,
+        ROSBridge::ComPrivate::MapStruct &mapWrapper,
         std::shared_ptr<WsClient::Connection>,
         std::shared_ptr<WsClient::InMessage> in_message)
 {

src/comm/ros_bridge/include/TopicSubscriber.h

@@ -10,7 +10,7 @@ namespace ComPrivate {
 
 
 typedef std::function<void(JsonDocUPtr)> CallbackType;
-struct CallbackMapWrapper{
+struct MapStruct{
     typedef std::map<HashType, CallbackType> Map;
     Map map;
     std::mutex mutex;
@@ -22,7 +22,7 @@ class TopicSubscriber
 public:
 
     TopicSubscriber() = delete;
-    TopicSubscriber(CasePacker *casePacker, RosbridgeWsClient *rbc, std::mutex *rbcMutex);
+    TopicSubscriber(CasePacker &casePacker, RosbridgeWsClient &rbc);
     ~TopicSubscriber();
 
     //! @brief Starts the subscriber.
@@ -40,16 +40,15 @@ private:
 
 
 
-    CasePacker         *_casePacker;
-    RosbridgeWsClient  *_rbc;
-    std::mutex         *_rbcMutex;
-    CallbackMapWrapper         _callbackMap;
+    CasePacker         &_casePacker;
+    RosbridgeWsClient  &_rbc;
+    MapStruct           _callbackMapStruct;
     ClientList          _clientList;
     bool                _running;
 };
 
 void subscriberCallback(const HashType &hash,
-                        CallbackMapWrapper &mapWrapper,
+                        MapStruct &mapWrapper,
                         std::shared_ptr<WsClient::Connection> /*connection*/,
                         std::shared_ptr<WsClient::InMessage> in_message);
 } // namespace ComPrivate

src/comm/ros_bridge/src/ROSBridge.cpp

@@ -3,8 +3,8 @@
 ROSBridge::ROSBridge::ROSBridge() :
     _casePacker(&_typeFactory, &_jsonFactory)
   , _rbc("localhost:9090")
-  , _topicPublisher(&_casePacker, &_rbc, &_rbcMutex)
-  , _topicSubscriber(&_casePacker, &_rbc, &_rbcMutex)
+  , _topicPublisher(_casePacker, _rbc)
+  , _topicSubscriber(_casePacker, _rbc)
 {
 
 }
@@ -38,7 +38,6 @@ void ROSBridge::ROSBridge::reset()
 
 bool ROSBridge::ROSBridge::connected()
 {
-    std::lock_guard<std::mutex> lk(_rbcMutex);
     return _rbc.connected();
 }