before restoring src/comm/ros_bridge/include

qgroundcontrol.pro

@@ -489,7 +489,7 @@ HEADERS += \
     src/comm/ros_bridge/include/MessageTag.h \
     src/comm/ros_bridge/include/MessageTraits.h \
     src/comm/ros_bridge/include/RosBridgeClient.h \
-    src/comm/ros_bridge/include/ThreadSafeQueue.h \
+    src/comm/ros_bridge/include/Server.h \
     src/comm/ros_bridge/include/TopicPublisher.h \
     src/comm/ros_bridge/include/TopicSubscriber.h \
     src/comm/ros_bridge/include/TypeFactory.h \
@@ -513,6 +513,7 @@ SOURCES += \
     src/Wima/WimaBridge.cc \
     src/comm/ros_bridge/include/ComPrivateInclude.cpp \
     src/comm/ros_bridge/include/MessageTag.cpp \
+    src/comm/ros_bridge/include/Server.cpp \
     src/comm/ros_bridge/include/TopicPublisher.cpp \
     src/comm/ros_bridge/include/TopicSubscriber.cpp \
     src/comm/ros_bridge/src/CasePacker.cpp \

src/Wima/WimaController.cc

@@ -35,9 +35,10 @@ const char* WimaController::snakeMinTileAreaName        = "SnakeMinTileArea";
 const char* WimaController::snakeLineDistanceName       = "SnakeLineDistance";
 const char* WimaController::snakeMinTransectLengthName  = "SnakeMinTransectLength";
 
-WimaController::StatusMap WimaController::_nemoStatusMap{std::make_pair<int, QString>(0, "No Heartbeat"),
-                                                         std::make_pair<int, QString>(1, "Connected"),
-                                                         std::make_pair<int, QString>(-1, "Timeout")};
+WimaController::StatusMap WimaController::_nemoStatusMap{
+    std::make_pair<int, QString>(0, "No Heartbeat"),
+    std::make_pair<int, QString>(1, "Connected"),
+    std::make_pair<int, QString>(-1, "Timeout")};
 
 using namespace snake;
 using namespace snake_geometry;
@@ -55,7 +56,9 @@ WimaController::WimaController(QObject *parent)
     , _snakeManager             (_managerSettings, _areaInterface)
     , _rtlManager               (_managerSettings, _areaInterface)
     , _currentManager           (&_defaultManager)
-    , _metaDataMap              (FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/WimaController.SettingsGroup.json"), this))
+    , _managerList              {&_defaultManager, &_snakeManager, &_rtlManager}
+    , _metaDataMap              (FactMetaData::createMapFromJsonFile(
+                                     QStringLiteral(":/json/WimaController.SettingsGroup.json"), this))
     , _enableWimaController     (settingsGroup, _metaDataMap[enableWimaControllerName])
     , _overlapWaypoints         (settingsGroup, _metaDataMap[overlapWaypointsName])
     , _maxWaypointsPerPhase     (settingsGroup, _metaDataMap[maxWaypointsPerPhaseName])
@@ -68,7 +71,6 @@ WimaController::WimaController(QObject *parent)
     , _measurementPathLength    (-1)
     , _lowBatteryHandlingTriggered  (false)
     , _snakeCalcInProgress      (false)
-    , _scenarioDefinedBool      (false)
     , _snakeTileWidth           (settingsGroup, _metaDataMap[snakeTileWidthName])
     , _snakeTileHeight          (settingsGroup, _metaDataMap[snakeTileHeightName])
     , _snakeMinTileArea         (settingsGroup, _metaDataMap[snakeMinTileAreaName])
@@ -89,9 +91,20 @@ WimaController::WimaController(QObject *parent)
     connect(&_arrivalReturnSpeed,           &Fact::rawValueChanged, this, &WimaController::_updateArrivalReturnSpeed);
     connect(&_altitude,                     &Fact::rawValueChanged, this, &WimaController::_updateAltitude);
 
-    _defaultManager.setOverlap(_overlapWaypoints.rawValue().toUInt());
-    _defaultManager.setN(_maxWaypointsPerPhase.rawValue().toUInt());
-    _defaultManager.setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt());
+    // Init waypoint managers.
+    bool value;
+    size_t overlap  = _overlapWaypoints.rawValue().toUInt(&value);
+    Q_ASSERT(value);
+    size_t N        = _maxWaypointsPerPhase.rawValue().toUInt(&value);
+    Q_ASSERT(value);
+    size_t startIdx = _nextPhaseStartWaypointIndex.rawValue().toUInt(&value);
+    Q_ASSERT(value);
+    (void)value;
+    for (auto manager : _managerList){
+        manager->setOverlap(overlap);
+        manager->setN(N);
+        manager->setStartIndex(startIdx);
+    }
 
     // Periodic.
     connect(&_eventTimer, &QTimer::timeout, this, &WimaController::_eventTimerHandler);
@@ -343,17 +356,8 @@ bool WimaController::_calcShortestPath(const QGeoCoordinate &start, const QGeoCo
     return  retVal;
 }
 
-/*!
- * \fn void WimaController::containerDataValidChanged(bool valid)
- * Pulls plan data generated by \c WimaPlaner from the \c _container if the data is valid (\a valid equals true).
- * Is connected to the dataValidChanged() signal of the \c WimaDataContainer.
- *
- * \sa WimaDataContainer, WimaPlaner, WimaPlanData
- */
 bool WimaController::setWimaPlanData(const WimaPlanData &planData)
 {
-    // fetch only if valid, return true on success
-
     // reset visual items
     _areas.clear();
     _defaultManager.clear();
@@ -779,17 +783,6 @@ void WimaController::_eventTimerHandler()
             _pRosBridge->reset();
             _bridgeSetupDone = false;
         }
-
-        //qWarning() << "Connectable: " << _pRosBridge->connected() << "\n";
-
-
-        //auto start = std::chrono::high_resolution_clock::now();
-
-
-
-        //auto end = std::chrono::high_resolution_clock::now();
-        //qWarning() << "Duration: " << std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count()
-                   //<< " ms\n";
     }
 }
 
@@ -838,14 +831,23 @@ void WimaController::_switchWaypointManager(WaypointManager::ManagerBase &manage
     if (_currentManager != &manager) {
         _currentManager = &manager;
 
-        bool value;
-        _currentManager->setN(_maxWaypointsPerPhase.rawValue().toUInt(&value));
-        Q_ASSERT(value);
-        _currentManager->setOverlap(_overlapWaypoints.rawValue().toUInt(&value));
-        Q_ASSERT(value);
-        _currentManager->setStartIndex(_nextPhaseStartWaypointIndex.rawValue().toUInt(&value)-1);
-        Q_ASSERT(value);
-        (void)value;
+        disconnect(&_overlapWaypoints,             &Fact::rawValueChanged,
+                   this, &WimaController::_updateOverlap);
+        disconnect(&_maxWaypointsPerPhase,         &Fact::rawValueChanged,
+                   this, &WimaController::_updateMaxWaypoints);
+        disconnect(&_nextPhaseStartWaypointIndex,  &Fact::rawValueChanged,
+                   this, &WimaController::_setStartIndex);
+
+        _maxWaypointsPerPhase.setRawValue(_currentManager->N());
+        _overlapWaypoints.setRawValue(_currentManager->overlap());
+        _nextPhaseStartWaypointIndex.setRawValue(_currentManager->startIndex());
+
+        connect(&_overlapWaypoints,             &Fact::rawValueChanged,
+                this, &WimaController::_updateOverlap);
+        connect(&_maxWaypointsPerPhase,         &Fact::rawValueChanged,
+                this, &WimaController::_updateMaxWaypoints);
+        connect(&_nextPhaseStartWaypointIndex,  &Fact::rawValueChanged,
+                this, &WimaController::_setStartIndex);
 
         emit missionItemsChanged();
         emit currentMissionItemsChanged();
@@ -888,33 +890,13 @@ void WimaController::_setSnakeCalcInProgress(bool inProgress)
     }
 }
 
-bool WimaController::_isScenarioDefined()
-{
-    _scenarioDefinedBool = _scenario.defined(_snakeTileWidth.rawValue().toDouble(),
-                                             _snakeTileHeight.rawValue().toDouble(),
-                                             _snakeMinTileArea.rawValue().toDouble());
-    return _scenarioDefinedBool;
-}
-
-bool WimaController::_isScenarioDefinedErrorMessage()
-{
-    bool value = _isScenarioDefined();
-    if (!value){
-        QString errorString;
-        for (auto c : _scenario.errorString)
-            errorString.push_back(c);
-        qgcApp()->showMessage(errorString);
-    }
-    return value;
-}
-
 void WimaController::_snakeStoreWorkerResults()
 {
+    _setSnakeCalcInProgress(false);
     auto start = std::chrono::high_resolution_clock::now();
     _snakeManager.clear();
 
     const WorkerResult_t &r = _snakeWorker.getResult();
-    _setSnakeCalcInProgress(false);
     if (!r.success) {
         //qgcApp()->showMessage(r.errorMessage);
         return;

src/Wima/WimaController.h

@@ -338,8 +338,6 @@ private slots:
     void    _smartRTLCleanUp                    (bool flying);
     // Snake.
     void _setSnakeCalcInProgress             (bool inProgress);
-    bool _isScenarioDefined                  (void);
-    bool _isScenarioDefinedErrorMessage      (void);
     void _snakeStoreWorkerResults            ();
     void _startStopRosBridge                 ();
     void _initStartSnakeWorker               ();
@@ -357,7 +355,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 +369,8 @@ private:
     WaypointManager::DefaultManager     _snakeManager;
     WaypointManager::RTLManager         _rtlManager;
     WaypointManager::ManagerBase       *_currentManager;
+    using ManagerList = QList<WaypointManager::ManagerBase*>;
+    ManagerList                         _managerList;
 
     // Settings Facts.
     QMap<QString, FactMetaData*> _metaDataMap;

src/comm/ros_bridge/include/Server.cpp

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

src/comm/ros_bridge/include/Server.h

+#ifndef SERVER_H
+#define SERVER_H
+
+
+class Server
+{
+public:
+    Server();
+};
+
+#endif // SERVER_H

src/comm/ros_bridge/include/ThreadSafeQueue.h

-#pragma once
-
-#include <queue>
-#include <mutex>
-#include <condition_variable>
-
-namespace ROSBridge {
-
-template <class T>
-class ThreadSafeQueue
-{
-public:
-    ThreadSafeQueue();
-    ~ThreadSafeQueue();
-
-    T pop_front();
-
-    void push_back(const T& item);
-    void push_back(T&& item);
-
-    int size();
-    bool empty();
-
-private:
-    std::deque<T> _queue;
-    std::mutex _mutex;
-    std::condition_variable _cond;
-};
-
-template <typename T>
-ThreadSafeQueue<T>::ThreadSafeQueue(){}
-
-template <typename T>
-ThreadSafeQueue<T>::~ThreadSafeQueue(){}
-
-template <typename T>
-T ThreadSafeQueue<T>::pop_front()
-{
-    std::unique_lock<std::mutex> mlock(_mutex);
-    while (_queue.empty())
-    {
-        _cond.wait(mlock);
-    }
-    T t = std::move(_queue.front());
-    _queue.pop_front();
-    return t;
-}
-
-template <typename T>
-void ThreadSafeQueue<T>::push_back(const T& item)
-{
-    std::unique_lock<std::mutex> mlock(_mutex);
-    _queue.push_back(item);
-    mlock.unlock();     // unlock before notificiation to minimize mutex con
-    _cond.notify_one(); // notify one waiting thread
-
-}
-
-template <typename T>
-void ThreadSafeQueue<T>::push_back(T&& item)
-{
-    std::unique_lock<std::mutex> mlock(_mutex);
-    _queue.push_back(std::move(item));
-    mlock.unlock();     // unlock before notificiation to minimize mutex con
-    _cond.notify_one(); // notify one waiting thread
-
-}
-
-template <typename T>
-int ThreadSafeQueue<T>::size()
-{
-    std::unique_lock<std::mutex> mlock(_mutex);
-    int size = _queue.size();
-    mlock.unlock();
-    return size;
-}
-
-} // namespace
-