refactoring WimaController, adding waypoint managers.

qgroundcontrol.pro

@@ -437,7 +437,9 @@ HEADERS += \
     src/Wima/QtROSTypeFactory.h \
     src/Wima/SnakeTiles.h \
     src/Wima/SnakeTilesLocal.h \
-    src/Wima/WimaControllerDetail.h \
+    src/Wima/SnakeWorker.h \
+    src/Wima/WaypointManager/GenericPathSlicer.h \
+    src/Wima/WaypointManager/GenericWaypointManager.h \
     src/Wima/WimaPolygonArray.h \
     src/Wima/snaketile.h \
     src/api/QGCCorePlugin.h \
@@ -493,12 +495,14 @@ SOURCES += \
     src/Wima/GeoPoint3D.cpp \
     src/Wima/PolygonArray.cc \
     src/Wima/QNemoProgress.cc \
+    src/Wima/SnakeWorker.cc \
+    src/Wima/WaypointManager/GenericPathSlicer.cpp \
+    src/Wima/WaypointManager/GenericWaypointManager.cpp \
     src/comm/ros_bridge/include/ComPrivateInclude.cpp \
     src/comm/ros_bridge/include/MessageTag.cpp \
     src/comm/ros_bridge/include/TopicPublisher.cpp \
     src/comm/ros_bridge/include/TopicSubscriber.cpp \
     src/comm/ros_bridge/src/CasePacker.cpp \
-    src/Wima/WimaControllerDetail.cc \
     src/Wima/snaketile.cpp \
     src/api/QGCCorePlugin.cc \
     src/api/QGCOptions.cc \

src/FlightDisplay/FlightDisplayViewMap.qml

@@ -244,9 +244,12 @@ FlightMap {
     // Add Snake tiles center points to the map
     MapItemView {
 
+        property bool _lengthMatching: wimaController.snakeTileCenterPoints.length
+                                       === wimaController.snakeProgress.length
 
         property bool _enable:    wimaController.enableWimaController.value
                                && wimaController.enableSnake.value
+                               && _lengthMatching
 
         model: _enable ? wimaController.snakeTileCenterPoints : 0
 

src/MissionManager/MissionController.cc

@@ -378,7 +378,7 @@ int MissionController::_nextSequenceNumber(void)
     }
 }
 
-int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i)
+int MissionController::insertSimpleMissionItem(const QGeoCoordinate &coordinate, int i)
 {
     int sequenceNumber = _nextSequenceNumber();
     SimpleMissionItem * newItem = new SimpleMissionItem(_controllerVehicle, _flyView, this);
@@ -410,6 +410,58 @@ int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i)
     return newItem->sequenceNumber();
 }
 
+int MissionController::insertSimpleMissionItems(const QList<QGeoCoordinate> &coordinates, int idx)
+{
+    MAV_CMD mavCmd = MAV_CMD_NAV_WAYPOINT;
+    if (    _visualItems->count() == 1
+        && (   _controllerVehicle->fixedWing()
+            || _controllerVehicle->vtol()
+            || _controllerVehicle->multiRotor()) )
+    {
+        mavCmd = _controllerVehicle->vtol() ? MAV_CMD_NAV_VTOL_TAKEOFF : MAV_CMD_NAV_TAKEOFF;
+        if ( !_controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains(mavCmd)) {
+            mavCmd = MAV_CMD_NAV_WAYPOINT;
+        }
+    }
+
+    int sequenceNumber = _nextSequenceNumber();
+    int size = coordinates.size();
+
+    SimpleMissionItem *newItem = nullptr;
+    bool firstItem = true;
+    double prevAltitude = 0.0;
+    int prevAltitudeMode = 0;
+    for (int i = 0; i < size; ++i) {
+        newItem = new SimpleMissionItem(_controllerVehicle, _flyView, this);
+        newItem->setSequenceNumber(sequenceNumber);
+        newItem->setCoordinate(coordinates[i]);
+        newItem->setCommand(mavCmd);
+        _initSimpleItem(newItem);
+        if (!firstItem || (  newItem->specifiesAltitude()
+                           && _findPreviousAltitude(i, &prevAltitude, &prevAltitudeMode)) )
+        {
+            newItem->altitude()->setRawValue(prevAltitude);
+            newItem->setAltitudeMode(static_cast<QGroundControlQmlGlobal::AltitudeMode>(prevAltitudeMode));
+        }
+        newItem->setMissionFlightStatus(_missionFlightStatus);
+        _visualItems->insert(idx++, newItem);
+        sequenceNumber = newItem->lastSequenceNumber() + 1;
+        mavCmd  = MAV_CMD_NAV_WAYPOINT;
+        firstItem = false;
+    }
+
+
+    _recalcSequence();
+    _recalcChildItems();
+    _recalcWaypointLines();
+    _updateTimer.start(UPDATE_TIMEOUT);
+
+    if ( newItem == nullptr)
+        return -1;
+    return newItem->sequenceNumber();
+
+}
+
 int MissionController::insertSimpleMissionItem(const MissionItem &missionItem, int i)
 {
     int sequenceNumber = _nextSequenceNumber();
@@ -1674,7 +1726,7 @@ void MissionController::_setPlannedHomePositionFromFirstCoordinate(const QGeoCoo
 }
 
 /// @param clickCoordinate The location of the user click when inserting a new item
-void MissionController::_recalcAllWithClickCoordinate(QGeoCoordinate& clickCoordinate)
+void MissionController::_recalcAllWithClickCoordinate(const QGeoCoordinate& clickCoordinate)
 {
     if (!_flyView) {
         _setPlannedHomePositionFromFirstCoordinate(clickCoordinate);
@@ -1770,37 +1822,61 @@ void MissionController::_deinitAllVisualItems(void)
 
 void MissionController::_initVisualItem(VisualMissionItem* visualItem)
 {
-    setDirty(false);
-
-    connect(visualItem, &VisualMissionItem::specifiesCoordinateChanged,                 this, &MissionController::_recalcWaypointLines);
-    connect(visualItem, &VisualMissionItem::coordinateHasRelativeAltitudeChanged,       this, &MissionController::_recalcWaypointLines);
-    connect(visualItem, &VisualMissionItem::exitCoordinateHasRelativeAltitudeChanged,   this, &MissionController::_recalcWaypointLines);
-    connect(visualItem, &VisualMissionItem::specifiedFlightSpeedChanged,                this, &MissionController::_recalcMissionFlightStatus);
-    connect(visualItem, &VisualMissionItem::specifiedGimbalYawChanged,                  this, &MissionController::_recalcMissionFlightStatus);
-    connect(visualItem, &VisualMissionItem::specifiedGimbalPitchChanged,                this, &MissionController::_recalcMissionFlightStatus);
-    connect(visualItem, &VisualMissionItem::terrainAltitudeChanged,                     this, &MissionController::_recalcMissionFlightStatus);
-    connect(visualItem, &VisualMissionItem::additionalTimeDelayChanged,                 this, &MissionController::_recalcMissionFlightStatus);
-    connect(visualItem, &VisualMissionItem::lastSequenceNumberChanged,                  this, &MissionController::_recalcSequence);
-
     if (visualItem->isSimpleItem()) {
-        // We need to track commandChanged on simple item since recalc has special handling for takeoff command
         SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(visualItem);
         if (simpleItem) {
-            connect(&simpleItem->missionItem()._commandFact, &Fact::valueChanged, this, &MissionController::_itemCommandChanged);
+            _initSimpleItem(simpleItem);
         } else {
             qWarning() << "isSimpleItem == true, yet not SimpleMissionItem";
         }
     } else {
         ComplexMissionItem* complexItem = qobject_cast<ComplexMissionItem*>(visualItem);
         if (complexItem) {
-            connect(complexItem, &ComplexMissionItem::complexDistanceChanged,       this, &MissionController::_recalcMissionFlightStatus);
-            connect(complexItem, &ComplexMissionItem::greatestDistanceToChanged,    this, &MissionController::_recalcMissionFlightStatus);
+            _initComplexItem(complexItem);
         } else {
             qWarning() << "ComplexMissionItem not found";
         }
     }
 }
 
+void MissionController::_initVisualItemCommon(VisualMissionItem *visualItem)
+{
+    setDirty(false);
+
+    connect(visualItem, &VisualMissionItem::specifiesCoordinateChanged,
+            this,       &MissionController::_recalcWaypointLines);
+    connect(visualItem, &VisualMissionItem::coordinateHasRelativeAltitudeChanged,
+            this,       &MissionController::_recalcWaypointLines);
+    connect(visualItem, &VisualMissionItem::exitCoordinateHasRelativeAltitudeChanged,
+            this,       &MissionController::_recalcWaypointLines);
+    connect(visualItem, &VisualMissionItem::specifiedFlightSpeedChanged,
+            this,       &MissionController::_recalcMissionFlightStatus);
+    connect(visualItem, &VisualMissionItem::specifiedGimbalYawChanged,
+            this,       &MissionController::_recalcMissionFlightStatus);
+    connect(visualItem, &VisualMissionItem::specifiedGimbalPitchChanged,
+            this,       &MissionController::_recalcMissionFlightStatus);
+    connect(visualItem, &VisualMissionItem::terrainAltitudeChanged,
+            this,       &MissionController::_recalcMissionFlightStatus);
+    connect(visualItem, &VisualMissionItem::additionalTimeDelayChanged,
+            this,       &MissionController::_recalcMissionFlightStatus);
+    connect(visualItem, &VisualMissionItem::lastSequenceNumberChanged,
+            this,       &MissionController::_recalcSequence);
+}
+
+void MissionController::_initSimpleItem(SimpleMissionItem *item)
+{
+    _initVisualItemCommon(item);
+    // We need to track commandChanged on simple item since recalc has special handling for takeoff command
+    connect(&item->missionItem()._commandFact, &Fact::valueChanged, this, &MissionController::_itemCommandChanged);
+}
+
+void MissionController::_initComplexItem(ComplexMissionItem *item)
+{
+    _initVisualItemCommon(item);
+    connect(item, &ComplexMissionItem::complexDistanceChanged,       this, &MissionController::_recalcMissionFlightStatus);
+    connect(item, &ComplexMissionItem::greatestDistanceToChanged,    this, &MissionController::_recalcMissionFlightStatus);
+}
+
 void MissionController::_deinitVisualItem(VisualMissionItem* visualItem)
 {
     // Disconnect all signals

src/MissionManager/MissionController.h

@@ -109,7 +109,12 @@ public:
     /// Add a new simple mission item to the list
     ///     @param i: index to insert at
     /// @return Sequence number for new item
-    Q_INVOKABLE int insertSimpleMissionItem(QGeoCoordinate coordinate, int i);
+    Q_INVOKABLE int insertSimpleMissionItem(const QGeoCoordinate &coordinate, int i);
+
+    /// Add a list of new simple mission item to the list.
+    ///     @param i: index to insert at
+    /// @return Sequence number of last item.
+    Q_INVOKABLE int insertSimpleMissionItems(const QList<QGeoCoordinate> &coordinates, int i);
 
     /// Add a new simple mission item to the list
     ///     @param i: index to insert at
@@ -289,10 +294,13 @@ private:
     void _init(void);
     void _recalcSequence(void);
     void _recalcChildItems(void);
-    void _recalcAllWithClickCoordinate(QGeoCoordinate& clickCoordinate);
+    void _recalcAllWithClickCoordinate(const QGeoCoordinate &clickCoordinate);
     void _initAllVisualItems(void);
     void _deinitAllVisualItems(void);
     void _initVisualItem(VisualMissionItem* item);
+    void _initVisualItemCommon(VisualMissionItem* visualItem); // Don't call this function, it is used by _initSimpleItem and _initComplexItem.
+    void _initSimpleItem(SimpleMissionItem* item);
+    void _initComplexItem(ComplexMissionItem* item);
     void _deinitVisualItem(VisualMissionItem* item);
     void _setupActiveVehicle(Vehicle* activeVehicle, bool forceLoadFromVehicle);
     void _calcPrevWaypointValues(double homeAlt, VisualMissionItem* currentItem, VisualMissionItem* prevItem, double* azimuth, double* distance, double* altDifference);

src/Snake/snake_geometry.cpp

@@ -274,7 +274,8 @@ void offsetPolygon(const BoostPolygon &polygon, BoostPolygon &polygonOffset, dou
 
     bg::buffer(polygon, result, distance_strategy, side_strategy, join_strategy, end_strategy, point_strategy);
 
-    polygonOffset = result[0];
+    if (result.size() > 0)
+        polygonOffset = result[0];
 
 }
 

src/Wima/QNemoProgress.cc

 #include "QNemoProgress.h"
 
-QNemoProgress::QNemoProgress(QObject *parent) :
-    ProgressBase()
-  , QObject(parent)
-{
-
-}
-
-QNemoProgress::QNemoProgress(const QNemoProgress &other, QObject *parent):
-    ProgressBase(other)
-  , QObject(parent)
-{
-
-}
-
-QNemoProgress *QNemoProgress::Clone() const {
-    return new QNemoProgress(*this, this->parent());
-}
-
-QVector<int> &QNemoProgress::progress() {
-    emit QNemoProgress::progressChanged();
-    return _progress;
-}
-
-const QVector<int> &QNemoProgress::progress() const {
-    return _progress;
-}

src/Wima/QNemoProgress.h

@@ -5,22 +5,6 @@
 #include "ros_bridge/include/GenericMessages.h"
 
 namespace NemoMsgs = ROSBridge::GenericMessages::NemoMsgs;
-typedef NemoMsgs::GenericProgress<int, QVector> ProgressBase;
-
-class QNemoProgress : public ProgressBase, public QObject {
-public:
-    QNemoProgress(QObject *parent = nullptr);
-    QNemoProgress(const QNemoProgress &other, QObject *parent = nullptr);
-
-
-
-    virtual QNemoProgress *Clone() const override;
-
-    virtual const QVector<int> &progress(void) const override;
-    virtual       QVector<int> &progress(void)       override;
-
-    signals:
-    void progressChanged();
-};
+typedef NemoMsgs::GenericProgress<int, QVector> QNemoProgress;
 
 

src/Wima/WimaControllerDetail.cc → src/Wima/SnakeWorker.cc

-#include "WimaControllerDetail.h"
+#include "SnakeWorker.h"
 #include <QGeoCoordinate>
 
 
@@ -6,7 +6,10 @@ SnakeWorker::SnakeWorker(QObject *parent) : QThread(parent)
   , _lineDistance       (3) // meter
   , _minTransectLength  (2) // meter
 {
+}
 
+SnakeWorker::~SnakeWorker()
+{
 }
 
 void SnakeWorker::setScenario(const Scenario &scenario)
@@ -80,6 +83,4 @@ void SnakeWorker::run()
         }
 
     }
-
-    emit resultReady();
 }

src/Wima/WimaControllerDetail.h → src/Wima/SnakeWorker.h

@@ -28,6 +28,7 @@ class SnakeWorker : public QThread{
 
 public:
     SnakeWorker(QObject *parent = nullptr);
+    ~SnakeWorker() override;
 
     void setScenario            (const Scenario  &scenario);
     void setProgress            (const QVector<int> &progress);
@@ -36,9 +37,6 @@ public:
 
     const WorkerResult_t &getResult() const;
 
-signals:
-    void resultReady();
-
 protected:
     void run() override;
 

src/Wima/WaypointManager/GenericPathSlicer.cpp

+#include "GenericPathSlicer.h"

src/Wima/WaypointManager/GenericPathSlicer.h

+#pragma once
+
+#include <assert.h>
+#include <iostream>
+
+//! @brief Base class for all waypoint managers.
+template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
+class GenericPathSlicer
+{
+public:
+    typedef Container<ElementType> ContainerType;
+    GenericPathSlicer();
+
+    const ContainerType &path  () const;
+
+    // Waypoint editing.
+    void            setPath     (const ContainerType &path);
+    void            push_back   (const ElementType &wp);
+    void            push_front  (const ElementType &wp);
+    void            clear       ();
+    void            insert      (int i, const ElementType &wp);
+    uint32_t size();
+    ElementType     &at         (unsigned int i);
+
+
+    // Slicing.
+    void slice      (ContainerType &slice);
+    void next       (ContainerType &slice);
+    void previous   (ContainerType &slice);
+    void reset      (ContainerType &slice);
+
+
+    // Slicing parameters.
+    //! @brief Sets the overlap.
+    //!
+    //! @param overlap The number of overlapping vertices
+    //! between on and the next slice.
+    void setOverlap            (uint32_t overlap);
+    //! @brief Sets the number of vertices per slice.
+    //!
+    //! @param N The number of vertices per slice.
+    void setN   (std::uint32_t N);
+    //! @brief Sets the start index.
+    //!
+    //! @param idxStart The start index.
+    void setStartIndex         (int idxStart);
+
+    //! @return Returns the overlap.
+    uint32_t      overlap       ();
+    //! @return Returns the number of vertices per slice N.
+    uint32_t      N             ();
+    //! @return Returns the start index.
+    int           startIndex    ();
+    //! @return Returns the end index.
+    int           endIndex    ();
+    //! @return Returns the start index of the next slice.
+    int           nextIndex    ();
+
+
+private:
+    void _updateIdx();
+
+    ContainerType _path;
+    long          _idxStart;
+    long          _idxEnd;
+    long          _idxNext;
+    long          _idxPrevious;
+    uint32_t      _overlap;
+    uint32_t      _N;
+    bool          _idxValid;
+    bool          _atEnd;
+
+
+};
+
+
+template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
+GenericPathSlicer<ElementType, Container>::GenericPathSlicer():
+    _idxValid(false)
+  , _atEnd(false)
+{}
+
+template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
+const Container<ElementType> &GenericPathSlicer<ElementType, Container>::path() const {
+    return _path;
+}
+
+template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
+void GenericPathSlicer<ElementType, Container>::setPath(const ContainerType &waypoints) {
+    _idxValid = false;
+    _path = waypoints;
+}
+
+template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
+void GenericPathSlicer<ElementType, Container>::push_back(const ElementType &wp) {
+    _idxValid = false;
+    _path.push_back(wp);
+}
+
+template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
+void GenericPathSlicer<ElementType, Container>::push_front(const ElementType &wp) {
+    _idxValid = false;
+    _path.push_front(wp);
+}
+
+template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
+void GenericPathSlicer<ElementType, Container>::clear(){
+    _idxValid = false;
+    _path.clear();
+}
+
+template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
+void GenericPathSlicer<ElementType, Container>::insert(int i, const ElementType &wp){
+    _idxValid = false;
+    _path.insert(i, wp);
+}
+
+template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
+std::uint32_t GenericPathSlicer<ElementType, Container>::size()
+{
+    return std::uint32_t(_path.size());
+}
+
+template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
+ElementType &GenericPathSlicer<ElementType, Container>::at(unsigned int i)
+{
+    return _path.at(i);
+}
+
+template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
+void GenericPathSlicer<ElementType, Container>::setOverlap(std::uint32_t overlap)
+{
+    _idxValid = false;
+    _overlap = overlap;
+}
+
+template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
+void GenericPathSlicer<ElementType, Container>::setN(uint32_t N)
+{
+    _idxValid = false;
+    _N = N > 0 ? N : 1;
+}
+
+template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
+void GenericPathSlicer<ElementType, Container>::setStartIndex(int idxStart)
+{
+    _idxValid = false;
+    _idxStart = idxStart;
+}
+
+template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
+uint32_t GenericPathSlicer<ElementType, Container>::overlap()
+{
+    return _overlap;
+}
+
+template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
+uint32_t GenericPathSlicer<ElementType, Container>::N()
+{
+    return _N;
+}
+
+template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
+int GenericPathSlicer<ElementType, Container>::startIndex()
+{
+    if (!_idxValid)
+        _updateIdx();
+    return _idxStart;
+}
+
+template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
+int GenericPathSlicer<ElementType, Container>::endIndex()
+{
+    if (!_idxValid)
+        _updateIdx();
+    return _idxEnd;
+}
+
+template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
+int GenericPathSlicer<ElementType, Container>::nextIndex()
+{
+    if (!_idxValid)
+        _updateIdx();
+    return _idxNext;
+}
+
+
+template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
+void GenericPathSlicer<ElementType, Container>::_updateIdx()
+{
+    _idxValid = true;
+    _atEnd    = false;
+    std::uint64_t size = _path.size();
+    if ( _idxStart >= size-1 ) {
+        _idxStart   = size-1;
+        _idxEnd     = _idxStart;
+        _idxNext    = _idxStart;
+        _atEnd      = true;
+        return;
+    }
+
+    _idxStart < 0 ? 0 : _idxStart;
+
+    _idxEnd = _idxStart + _N - 1;
+    _idxEnd = _idxEnd < size ? _idxEnd : size-1;
+
+    _idxNext = _idxEnd + 1 - _overlap;
+    _idxNext = _idxNext < 0    ? 0        : _idxNext;
+    _idxNext = _idxNext < size ? _idxNext : size-1;
+
+    _idxPrevious = _idxStart - 1 + _overlap;
+    _idxPrevious = _idxPrevious < 0    ? 0        : _idxPrevious;
+    _idxPrevious = _idxPrevious < size ? _idxPrevious : size-1;
+}
+
+template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
+void GenericPathSlicer<ElementType, Container>::slice(Container<ElementType> &c){
+
+    if ( !_idxValid)
+        _updateIdx();
+
+    (void)c;
+    assert(false); //ToDo
+
+    // extract waypoints
+
+}
+
+template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
+void GenericPathSlicer<ElementType, Container>::next(Container<ElementType> &c){
+    setStartIndex(_idxNext);
+    slice(c);
+}
+
+template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
+void GenericPathSlicer<ElementType, Container>::previous(Container<ElementType> &c){
+    setStartIndex(_idxPrevious);
+    slice(c);
+}
+
+template <class ElementType, template <class, class...> class Container /*e.g. QVector*/>
+void GenericPathSlicer<ElementType, Container>::reset(Container<ElementType> &c){
+    setStartIndex(0);
+    slice(c);
+}

src/Wima/WaypointManager/GenericWaypointManager.cpp

+#include "GenericWaypointManager.h"
+
+GenericWaypointManager::GenericWaypointManager()
+{
+
+}

src/Wima/WaypointManager/GenericWaypointManager.h

+#pragma once
+
+
+class GenericWaypointManager
+{
+public:
+    GenericWaypointManager();
+};

src/Wima/WimaController.cc

@@ -17,7 +17,7 @@
 #include <QScopedPointer>
 
 
-const char* WimaController::wimaFileExtension           = "wima";
+// const char* WimaController::wimaFileExtension           = "wima";
 const char* WimaController::areaItemsName               = "AreaItems";
 const char* WimaController::missionItemsName            = "MissionItems";
 const char* WimaController::settingsGroup               = "WimaController";
@@ -101,29 +101,32 @@ WimaController::WimaController(QObject *parent)
     _enableDisableLowBatteryHandling(enableLowBatteryHandling->rawValue());
 
     // Snake Worker Thread.
-    connect(&_snakeWorker, &SnakeWorker::resultReady, this, &WimaController::_snakeStoreWorkerResults);
+    connect(&_snakeWorker, &SnakeWorker::finished, this, &WimaController::_snakeStoreWorkerResults);
+    connect(this, &WimaController::nemoProgressChanged, this, &WimaController::_initStartSnakeWorker);
+    connect(this, &QObject::destroyed, &this->_snakeWorker, &SnakeWorker::quit);
 
     // Start, stop RosBridge.
     connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_startStopRosBridge);
+    connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_initStartSnakeWorker);
     _startStopRosBridge();
 }
 
-QStringList WimaController::loadNameFilters() const
-{
-    QStringList filters;
+//QStringList WimaController::loadNameFilters() const
+//{
+//    QStringList filters;
 
-    filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension) <<
-               tr("All Files (*.*)");
-    return filters;
-}
+//    filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension) <<
+//               tr("All Files (*.*)");
+//    return filters;
+//}
 
-QStringList WimaController::saveNameFilters() const
-{
-    QStringList filters;
+//QStringList WimaController::saveNameFilters() const
+//{
+//    QStringList filters;
 
-    filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension);
-    return filters;
-}
+//    filters << tr("Supported types (*.%1 *.%2)").arg(wimaFileExtension).arg(AppSettings::planFileExtension);
+//    return filters;
+//}
 
 bool WimaController::uploadOverrideRequired() const
 {
@@ -331,37 +334,32 @@ void WimaController::removeVehicleTrajectoryHistory()
     managerVehicle->trajectoryPoints()->clear();
 }
 
-void WimaController::saveToCurrent()
-{
+//void WimaController::saveToFile(const QString& filename)
+//{
+//    QString file = filename;
+//}
 
-}
+//bool WimaController::loadFromCurrent()
+//{
+//    return true;
+//}
 
-void WimaController::saveToFile(const QString& filename)
-{
-    QString file = filename;
-}
+//bool WimaController::loadFromFile(const QString &filename)
+//{
+//    QString file = filename;
+//    return true;
+//}
 
-bool WimaController::loadFromCurrent()
-{
-    return true;
-}
-
-bool WimaController::loadFromFile(const QString &filename)
-{
-    QString file = filename;
-    return true;
-}
 
 
+//QJsonDocument WimaController::saveToJson(FileType fileType)
+//{
+//    if(fileType)
+//    {
 
-QJsonDocument WimaController::saveToJson(FileType fileType)
-{
-    if(fileType)
-    {
-
-    }
-    return QJsonDocument();
-}
+//    }
+//    return QJsonDocument();
+//}
 
 bool WimaController::calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector<QGeoCoordinate> &path)
 {
@@ -1002,31 +1000,21 @@ void WimaController::_eventTimerHandler()
         _checkBatteryLevel();
 
     // Snake flight plan update necessary?
-    if ( snakeEventLoopTicker.ready() ) {
-        if ( _enableSnake.rawValue().toBool() && _localPlanDataValid && !_snakeCalcInProgress && _scenarioDefinedBool) {
-
-            _snakeWorker.setScenario(_scenario);
-            _snakeWorker.setProgress(_snakeProgress.progress());
-            _snakeWorker.setLineDistance(_snakeLineDistance.rawValue().toDouble());
-            _snakeWorker.setMinTransectLength(_snakeMinTransectLength.rawValue().toDouble());
-            _setSnakeCalcInProgress(true);
-            _snakeWorker.start();
-
-            emit snakeProgressChanged();
-        }
-    }
+//    if ( snakeEventLoopTicker.ready() ) {
+//        if ( _enableSnake.rawValue().toBool() && _localPlanDataValid && !_snakeCalcInProgress && _scenarioDefinedBool) {
+//        }
+//    }
 
     if (rosBridgeTicker.ready()) {
-//        using namespace ros_bridge;
 
         _pRosBridge->publish(_snakeTilesLocal, "/snake/tiles");
         _pRosBridge->publish(_snakeOrigin, "/snake/origin");
 
         using namespace std::placeholders;
-        auto callBack = std::bind(ProgressFromJson,
-                                  std::ref(*(_pRosBridge->casePacker())),
+        auto callBack = std::bind(&WimaController::_progressFromJson,
+                                  this,
                                   _1,
-                                  std::ref(_snakeProgress));
+                                  std::ref(_nemoProgress));
         _pRosBridge->subscribe("/nemo/progress", callBack);
     }
 }
@@ -1372,13 +1360,34 @@ void WimaController::_snakeStoreWorkerResults()
 
 void WimaController::_startStopRosBridge()
 {
-    if (_enableSnake.rawValue().toBool()) {
+    if ( _enableSnake.rawValue().toBool() ) {
         _pRosBridge->start();
     } else {
-        _pRosBridge->stop();
+        _pRosBridge->reset();
     }
 }
 
+void WimaController::_initStartSnakeWorker()
+{
+    if ( !_enableSnake.rawValue().toBool() )
+        return;
+
+    // Stop worker thread if running.
+    if ( _snakeWorker.isRunning() ) {
+        _snakeWorker.quit();
+    }
+
+    // Initialize _snakeWorker.
+    _snakeWorker.setScenario(_scenario);
+    _snakeWorker.setProgress(_nemoProgress.progress());
+    _snakeWorker.setLineDistance(_snakeLineDistance.rawValue().toDouble());
+    _snakeWorker.setMinTransectLength(_snakeMinTransectLength.rawValue().toDouble());
+    _setSnakeCalcInProgress(true);
+
+    // Start worker thread.
+    _snakeWorker.start();
+}
+
 void WimaController::_loadCurrentMissionItemsFromBuffer()
 {
     _currentMissionItems.clear();
@@ -1398,7 +1407,14 @@ void WimaController::_saveCurrentMissionItemsToBuffer()
         _missionItemsBuffer.append(_currentMissionItems.removeAt(0));
 }
 
-void ProgressFromJson(const ROSBridge::CasePacker &casePacker, JsonDocUPtr pDoc, QNemoProgress &progress)
+void WimaController::_progressFromJson(JsonDocUPtr pDoc,
+                                      QNemoProgress &progress)
 {
-    casePacker.unpack(pDoc, progress);
+    int requiredSize = _snakeTilesLocal.polygons().size();
+    if ( !_pRosBridge->casePacker()->unpack(pDoc, progress)
+         || progress.progress().size() != requiredSize ) {
+        progress.progress().fill(0, requiredSize);
+    }
+
+    emit WimaController::nemoProgressChanged();
 }

src/Wima/WimaController.h

@@ -27,7 +27,7 @@
 #include "SettingsManager.h"
 
 #include "snake.h"
-#include "WimaControllerDetail.h"
+#include "SnakeWorker.h"
 #include "SnakeTiles.h"
 #include "SnakeTilesLocal.h"
 #include "GeoPoint3D.h"
@@ -65,10 +65,10 @@ public:
     Q_PROPERTY(PlanMasterController*    masterController        READ masterController       WRITE setMasterController       NOTIFY masterControllerChanged)
     Q_PROPERTY(MissionController*       missionController       READ missionController      WRITE setMissionController      NOTIFY missionControllerChanged)
     Q_PROPERTY(QmlObjectListModel*      visualItems             READ visualItems                                            NOTIFY visualItemsChanged)
-    Q_PROPERTY(QString                  currentFile             READ currentFile                                            NOTIFY currentFileChanged)
-    Q_PROPERTY(QStringList              loadNameFilters         READ loadNameFilters                                        CONSTANT)
-    Q_PROPERTY(QStringList              saveNameFilters         READ saveNameFilters                                        CONSTANT)
-    Q_PROPERTY(QString                  fileExtension           READ fileExtension                                          CONSTANT)
+//    Q_PROPERTY(QString                  currentFile             READ currentFile                                            NOTIFY currentFileChanged)
+//    Q_PROPERTY(QStringList              loadNameFilters         READ loadNameFilters                                        CONSTANT)
+//    Q_PROPERTY(QStringList              saveNameFilters         READ saveNameFilters                                        CONSTANT)
+//    Q_PROPERTY(QString                  fileExtension           READ fileExtension                                          CONSTANT)
     Q_PROPERTY(WimaDataContainer*       dataContainer           READ dataContainer          WRITE setDataContainer          NOTIFY dataContainerChanged)
     Q_PROPERTY(QmlObjectListModel*      missionItems            READ missionItems                                           NOTIFY missionItemsChanged)
     Q_PROPERTY(QmlObjectListModel*      currentMissionItems     READ currentMissionItems                                    NOTIFY currentMissionItemsChanged)
@@ -100,7 +100,7 @@ public:
     Q_PROPERTY(Fact*                    snakeMinTransectLength      READ snakeMinTransectLength     CONSTANT)
     Q_PROPERTY(QmlObjectListModel*      snakeTiles                  READ snakeTiles                 NOTIFY snakeTilesChanged)
     Q_PROPERTY(QVariantList             snakeTileCenterPoints       READ snakeTileCenterPoints      NOTIFY snakeTileCenterPointsChanged)
-    Q_PROPERTY(QVector<int>             *snakeProgress              READ snakeProgress              NOTIFY snakeProgressChanged)
+    Q_PROPERTY(QVector<int>             nemoProgress                READ nemoProgress               NOTIFY nemoProgressChanged)
 
 
 
@@ -108,10 +108,10 @@ public:
     PlanMasterController*       masterController       (void)           { return _masterController; }
     MissionController*          missionController      (void)           { return _missionController; }
     QmlObjectListModel*         visualItems            (void)           { return &_visualItems; }
-    QString                     currentFile            (void) const     { return _currentFile; }
-    QStringList                 loadNameFilters        (void) const;
-    QStringList                 saveNameFilters        (void) const;
-    QString                     fileExtension          (void) const     { return wimaFileExtension; }
+//    QString                     currentFile            (void) const     { return _currentFile; }
+//    QStringList                 loadNameFilters        (void) const;
+//    QStringList                 saveNameFilters        (void) const;
+//    QString                     fileExtension          (void) const     { return wimaFileExtension; }
     QGCMapPolygon               joinedArea             (void) const;
     WimaDataContainer*          dataContainer          (void)           { return _container; }
     QmlObjectListModel*         missionItems           (void)           { return &_missionItems; }
@@ -137,7 +137,7 @@ public:
     Fact*                       snakeMinTransectLength (void)           { return &_snakeMinTransectLength;}
     QmlObjectListModel*         snakeTiles             (void)           { return _snakeTiles.QmlObjectListModel();}
     QVariantList                snakeTileCenterPoints  (void)           { return _snakeTileCenterPoints;}
-    QVector<int>               *snakeProgress          (void)           { return &_snakeProgress.progress();}
+    QVector<int>                nemoProgress           (void)           { return _nemoProgress.progress();}
 
     bool                        uploadOverrideRequired (void) const;
     double                      phaseDistance          (void) const;
@@ -168,14 +168,14 @@ public:
     Q_INVOKABLE void removeVehicleTrajectoryHistory();
 
 
-    Q_INVOKABLE void saveToCurrent  ();
-    Q_INVOKABLE void saveToFile     (const QString& filename);
-    Q_INVOKABLE bool loadFromCurrent();
-    Q_INVOKABLE bool loadFromFile   (const QString& filename);
+//    Q_INVOKABLE void saveToCurrent  ();
+//    Q_INVOKABLE void saveToFile     (const QString& filename);
+//    Q_INVOKABLE bool loadFromCurrent();
+//    Q_INVOKABLE bool loadFromFile   (const QString& filename);
 
 
     // static Members
-    static const char* wimaFileExtension;
+//    static const char* wimaFileExtension;
     static const char* areaItemsName;
     static const char* missionItemsName;    
     static const char* settingsGroup;
@@ -213,7 +213,7 @@ signals:
     void masterControllerChanged            (void);
     void missionControllerChanged           (void);
     void visualItemsChanged                 (void);
-    void currentFileChanged                 ();
+//    void currentFileChanged                 ();
     void dataContainerChanged               ();
     void readyForSaveSendChanged            (bool ready);
     void missionItemsChanged                (void);
@@ -230,7 +230,7 @@ signals:
     void snakeCalcInProgressChanged         (void);
     void snakeTilesChanged                  (void);
     void snakeTileCenterPointsChanged       (void);
-    void snakeProgressChanged               (void);
+    void nemoProgressChanged                (void);
 private:
     enum SRTL_Reason {BatteryLow, UserRequest};
 private slots:
@@ -257,7 +257,7 @@ private slots:
     bool _verifyScenarioDefinedWithErrorMessage    (void);
     void _snakeStoreWorkerResults            ();
     void _startStopRosBridge                 ();
-
+    void _initStartSnakeWorker               ();
 
 private:
     void _setPhaseDistance(double distance);
@@ -268,9 +268,12 @@ private:
     void _loadCurrentMissionItemsFromBuffer();
     void _saveCurrentMissionItemsToBuffer();
 
+    void _progressFromJson(JsonDocUPtr pDoc,
+                           QNemoProgress &progress);
+
     PlanMasterController   *_masterController;
     MissionController      *_missionController;
-    QString                 _currentFile; // file for saveing
+//    QString                 _currentFile; // file for saveing
     WimaDataContainer      *_container; // container for data exchange with WimaController
     QmlObjectListModel      _visualItems; // contains all visible areas
     WimaJoinedAreaData      _joinedArea; // joined area fromed by opArea, serArea, _corridor
@@ -339,14 +342,12 @@ private:
     SnakeTiles                   _snakeTiles; // tiles
     SnakeTilesLocal              _snakeTilesLocal; // tiles local coordinate system
     QVariantList                 _snakeTileCenterPoints;
-    QNemoProgress                _snakeProgress; // measurement progress
+    QNemoProgress                _nemoProgress; // measurement progress
 
     ROSBridgePtr                 _pRosBridge;
 
 };
 
-void ProgressFromJson(const ROSBridge::CasePacker &casePacker, JsonDocUPtr pDoc, QNemoProgress &progress);
-
 /*
  * The following explains the structure of
  * _missionController.visualItems().

src/Wima/WimaPlaner.cc

@@ -562,7 +562,7 @@ bool WimaPlaner::calcArrivalAndReturnPath()
     _arrivalPathLength = path.size()-1;
     int sequenceNumber = 0;
     for (int i = 1; i < path.count()-1; i++) {
-        sequenceNumber = _missionController->insertSimpleMissionItem(path.value(i), missionItems->count()-1);
+        sequenceNumber = _missionController->insertSimpleMissionItem(path[i], missionItems->count()-1);
         _missionController->setCurrentPlanViewIndex(sequenceNumber, true);
     }
 
@@ -576,7 +576,7 @@ bool WimaPlaner::calcArrivalAndReturnPath()
     }
     _returnPathLength = path.size()-1; // -1: fist item is last measurement point
     for (int i = 1; i < path.count()-1; i++) {
-        sequenceNumber = _missionController->insertSimpleMissionItem(path.value(i), missionItems->count());
+        sequenceNumber = _missionController->insertSimpleMissionItem(path[i], missionItems->count());
         _missionController->setCurrentPlanViewIndex(sequenceNumber, true);
     }
 

src/comm/ros_bridge/include/CasePacker.h

@@ -30,7 +30,7 @@ public:
     template<class T>
     bool unpack(JsonDocUPtr &pDoc, T &msg) const {
         removeTag(pDoc);
-        return _typeFactory->create(pDoc, msg);
+        return _typeFactory->create(*pDoc.get(), msg);
     }
 
 

src/comm/ros_bridge/include/GenericMessages.h

@@ -331,6 +331,7 @@ public:
     GenericProgress(){}
     GenericProgress(const ContainterType<IntType> &progress) :_progress(progress){}
     GenericProgress(const GenericProgress &p) :_progress(p.progress()){}
+    ~GenericProgress() {}
 
     virtual GenericProgress *Clone() const override { return new GenericProgress(*this); }
 

src/comm/ros_bridge/include/ROSBridge.h

@@ -28,12 +28,14 @@ public:
     }
     void publish(JsonDocUPtr doc);
 
-    void subscribe(const char *topic, const std::function<(JsonDocUPtr)> &callBack);
+    void subscribe(const char *topic, const std::function<void(JsonDocUPtr)> &callBack);
 
     const CasePacker *casePacker() const;
 
+    //! @brief Start ROS bridge.
     void start();
-    void stop();
+    //! @brief Reset ROS bridge.
+    void reset();
 
 private:
     TypeFactory             _typeFactory;

src/comm/ros_bridge/include/TopicPublisher.cpp

 #include "TopicPublisher.h"
 
 
+
+ROSBridge::ComPrivate::TopicPublisher::TopicPublisher(CasePacker *casePacker,
+                                                      RosbridgeWsClient *rbc) :
+      _running(false)
+    , _casePacker(casePacker)
+    , _rbc(rbc)
+{
+
+}
+
+ROSBridge::ComPrivate::TopicPublisher::~TopicPublisher()
+{
+    this->reset();
+}
+
+void ROSBridge::ComPrivate::TopicPublisher::start()
+{
+    if ( _running.load() )  // start called while thread running.
+        return;
+    _running.store(true);
+    _rbc->addClient(ROSBridge::ComPrivate::_topicAdvertiserKey);
+    _pThread.reset(new std::thread(&ROSBridge::ComPrivate::transmittLoop,
+                                   std::cref(*_casePacker),
+                                   std::ref(*_rbc),
+                                   std::ref(_queue),
+                                   std::ref(_queueMutex),
+                                   std::ref(_advertisedTopicsHashList),
+                                   std::cref(_running)));
+}
+
+void ROSBridge::ComPrivate::TopicPublisher::reset()
+{
+    if ( !_running.load() )  // stop called while thread not running.
+        return;
+    _running.store(false);
+    if ( !_pThread )
+        return;
+    _pThread->join();
+    _pThread.reset();    
+    _rbc->removeClient(ROSBridge::ComPrivate::_topicAdvertiserKey);
+    _queue.clear();
+    _advertisedTopicsHashList.clear();
+}
+
 void ROSBridge::ComPrivate::transmittLoop(const ROSBridge::CasePacker &casePacker,
                                           RosbridgeWsClient &rbc,
                                           ROSBridge::ComPrivate::JsonQueue &queue,
                                           std::mutex &queueMutex,
                                           HashSet &advertisedTopicsHashList,
-                                          const std::atomic<bool> &stopFlag)
+                                          const std::atomic<bool> &running)
 {
     rbc.addClient(ROSBridge::ComPrivate::_topicPublisherKey);
 
-    while(!stopFlag.load()){
+    while(running.load()){
         // Pop message from queue.
         queueMutex.lock();
         //std::cout << "Queue size: " << queue.size() << std::endl;
@@ -55,43 +99,3 @@ void ROSBridge::ComPrivate::transmittLoop(const ROSBridge::CasePacker &casePacke
     } // while loop
 }
 
-ROSBridge::ComPrivate::TopicPublisher::TopicPublisher(CasePacker *casePacker,
-                                                      RosbridgeWsClient *rbc) :
-      _stopFlag(true)
-    , _casePacker(casePacker)
-    , _rbc(rbc)
-{
-
-}
-
-ROSBridge::ComPrivate::TopicPublisher::~TopicPublisher()
-{
-    this->stop();
-}
-
-void ROSBridge::ComPrivate::TopicPublisher::start()
-{
-    if ( !_stopFlag.load() )  // start called while thread running.
-        return;
-    _stopFlag.store(false);    
-    _rbc->addClient(ROSBridge::ComPrivate::_topicAdvertiserKey);
-    _pThread.reset(new std::thread(&ROSBridge::ComPrivate::transmittLoop,
-                                   std::cref(*_casePacker),
-                                   std::ref(*_rbc),
-                                   std::ref(_queue),
-                                   std::ref(_queueMutex),
-                                   std::ref(_advertisedTopicsHashList),
-                                   std::cref(_stopFlag)));
-}
-
-void ROSBridge::ComPrivate::TopicPublisher::stop()
-{
-    if ( _stopFlag.load() )  // start called while thread not running.
-        return;
-    _stopFlag.store(true);
-    if ( !_pThread )
-        return;
-    _pThread->join();
-    _pThread.reset();    
-    _rbc->removeClient(ROSBridge::ComPrivate::_topicAdvertiserKey);
-}

src/comm/ros_bridge/include/TopicPublisher.h

@@ -25,8 +25,12 @@ public:
 
     ~TopicPublisher();
 
+    //! @brief Starts the publisher.
     void start();
-    void stop();
+
+    //! @brief Resets the publisher.
+    void reset();
+
     template<class T>
     void publish(const T &msg, const std::string &topic){
         JsonDocUPtr docPtr(_casePacker->pack(msg, topic));
@@ -40,7 +44,7 @@ public:
 private:
     JsonQueue           _queue;
     std::mutex          _queueMutex;
-    std::atomic<bool>   _stopFlag;
+    std::atomic<bool>   _running;
     CasePacker         *_casePacker;
     ThreadPtr           _pThread;
     RosbridgeWsClient  *_rbc;
@@ -54,7 +58,7 @@ void transmittLoop(const ROSBridge::CasePacker &casePacker,
                    ROSBridge::ComPrivate::JsonQueue &queue,
                    std::mutex &queueMutex,
                    HashSet &advertisedTopicsHashList,
-                   const std::atomic<bool> &stopFlag);
+                   const std::atomic<bool> &running);
 
 
 } // namespace CommunicatorDetail

src/comm/ros_bridge/include/TopicSubscriber.cpp

@@ -6,32 +6,64 @@ ROSBridge::ComPrivate::TopicSubscriber::TopicSubscriber(
         RosbridgeWsClient *rbc) :
     _casePacker(casePacker)
   , _rbc(rbc)
+  , _running(false)
 {
 
 }
 
+ROSBridge::ComPrivate::TopicSubscriber::~TopicSubscriber()
+{
+    this->reset();
+}
+
 void ROSBridge::ComPrivate::TopicSubscriber::start()
 {
-    _rbc->addClient(ROSBridge::ComPrivate::_topicSubscriberKey);
+    _running = true;
 }
 
-void ROSBridge::ComPrivate::TopicSubscriber::stop()
+void ROSBridge::ComPrivate::TopicSubscriber::reset()
 {
-    _rbc->removeClient(ROSBridge::ComPrivate::_topicSubscriberKey);
+    if ( _running ){
+        for (std::string clientName : _clientList)
+            _rbc->removeClient(clientName);
+        _running = false;
+        _callbackMap.clear();
+        _clientList.clear();
+    }
 }
 
 bool ROSBridge::ComPrivate::TopicSubscriber::subscribe(
         const char *topic,
         const std::function<void(JsonDocUPtr)> &callback)
 {
-    using namespace std::placeholders;
-    HashType hash = getHash(topic);
+    if ( !_running )
+        return false;
+
+    std::string clientName = ROSBridge::ComPrivate::_topicSubscriberKey
+                           + std::string(topic);
+    _clientList.push_back(clientName);
+
+    HashType hash = getHash(clientName);
     auto ret = _callbackMap.insert(std::make_pair(hash, callback)); //
     if ( !ret.second )
         return false; // Topic subscription already present.
 
-    auto f = std::bind(&ROSBridge::ComPrivate::subscriberCallback, hash, _callbackMap, _1, _2);
-    _rbc->subscribe(ROSBridge::ComPrivate::_topicSubscriberKey, topic, f);
+    {
+    using namespace std::placeholders;
+    auto f = std::bind(&ROSBridge::ComPrivate::subscriberCallback,
+                      hash,
+                      std::cref(_callbackMap),
+                      _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;
+    _rbc->addClient(clientName);
+    _rbc->subscribe(clientName,
+                    topic,
+                    f );
+    }
 
     return true;
 }
@@ -78,3 +110,13 @@ void ROSBridge::ComPrivate::subscriberCallback(
     return;
 
 }
+
+void ROSBridge::ComPrivate::test(
+        std::shared_ptr<WsClient::Connection>,
+        std::shared_ptr<WsClient::InMessage> in_message)
+{
+
+    std::cout << "test: Json document: "
+              << in_message->string()
+              << std::endl;
+}

src/comm/ros_bridge/include/TopicSubscriber.h

@@ -10,15 +10,20 @@ namespace ComPrivate {
 
 class TopicSubscriber
 {
+    typedef std::vector<std::string> ClientList;
 public:    
     typedef std::function<void(JsonDocUPtr)> CallbackType;
     typedef std::map<HashType, CallbackType> CallbackMap;
 
     TopicSubscriber() = delete;
     TopicSubscriber(CasePacker *casePacker, RosbridgeWsClient *rbc);
+    ~TopicSubscriber();
 
+    //! @brief Starts the subscriber.
     void start();
-    void stop();
+
+    //! @brief Resets the subscriber.
+    void reset();
 
     //! @return Returns false if a subscription to this topic allready exists.
     //!
@@ -29,6 +34,8 @@ private:
     CasePacker         *_casePacker;
     RosbridgeWsClient  *_rbc;
     CallbackMap         _callbackMap;
+    ClientList          _clientList;
+    bool                _running;
 };
 
 void subscriberCallback(const HashType &hash,
@@ -36,5 +43,7 @@ void subscriberCallback(const HashType &hash,
                         std::shared_ptr<WsClient::Connection> /*connection*/,
                         std::shared_ptr<WsClient::InMessage> in_message);
 
-}
-}
+void test(std::shared_ptr<WsClient::Connection> /*connection*/,
+                        std::shared_ptr<WsClient::InMessage> in_message);
+} // namespace ComPrivate
+} // namespace ROSBridge

src/comm/ros_bridge/src/ROSBridge.cpp

@@ -30,9 +30,9 @@ void ROSBridge::ROSBridge::start()
     _topicSubscriber.start();
 }
 
-void ROSBridge::ROSBridge::stop()
+void ROSBridge::ROSBridge::reset()
 {
-    _topicPublisher.stop();
-    _topicSubscriber.stop();
+    _topicPublisher.reset();
+    _topicSubscriber.reset();
 }