temp

qgroundcontrol.pro

@@ -35,7 +35,7 @@ DebugBuild {
     #DEFINES += SNAKE_SHOW_TIME
     #DEFINES += SNAKE_DEBUG
     #DEFINES += SNAKE_SHOW_TIME
-    DEFINES += ROS_BRIDGE_DEBUG
+    #DEFINES += ROS_BRIDGE_DEBUG
 }
 else {
     DESTDIR  = $${OUT_PWD}/release
@@ -453,10 +453,12 @@ HEADERS += \
     src/MeasurementComplexItem/geometry/TileDiff.h \
     src/MeasurementComplexItem/geometry/geometry.h \
     src/MeasurementComplexItem/HashFunctions.h \
+    src/MeasurementComplexItem/nemo_interface/FutureWatcher.h \
+    src/MeasurementComplexItem/nemo_interface/FutureWatcherInterface.h \
     src/MeasurementComplexItem/nemo_interface/Task.h \
+    src/MeasurementComplexItem/nemo_interface/TaskDispatcher.h \
     src/MeasurementComplexItem/nemo_interface/tileHelper.h \
     src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h \
-    src/MeasurementComplexItem/nemo_interface/CommandDispatcher.h \
     src/MeasurementComplexItem/nemo_interface/MeasurementTile.h \
     src/QmlControls/QmlUnitsConversion.h \
     src/MeasurementComplexItem/geometry/GeoArea.h \
@@ -510,6 +512,7 @@ HEADERS += \
     src/comm/ros_bridge/include/message_traits.h \
     src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h \
     src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h \
+    src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.h \
     src/comm/ros_bridge/include/messages/nemo_msgs/tile.h \
     src/comm/ros_bridge/include/messages/std_msgs/header.h \
     src/comm/ros_bridge/include/server.h \
@@ -529,9 +532,10 @@ SOURCES += \
     src/MeasurementComplexItem/geometry/SafeArea.cc \
     src/MeasurementComplexItem/geometry/geometry.cpp \
     src/MeasurementComplexItem/HashFunctions.cpp \
-    src/MeasurementComplexItem/nemo_interface/CommandDispatcher.cpp \
+    src/MeasurementComplexItem/nemo_interface/FutureWatcherInterface.cpp \
     src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp \
     src/MeasurementComplexItem/nemo_interface/Task.cpp \
+    src/MeasurementComplexItem/nemo_interface/TaskDispatcher.cpp \
     src/Vehicle/VehicleEscStatusFactGroup.cc \
     src/MeasurementComplexItem/AreaData.cc \
     src/api/QGCCorePlugin.cc \
@@ -553,6 +557,7 @@ SOURCES += \
     src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.cpp \
     src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.cpp \
     src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.cpp \
+    src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.cpp \
     src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp \
     src/comm/ros_bridge/include/messages/std_msgs/header.cpp \
     src/comm/ros_bridge/include/messages/std_msgs/time.cpp \

src/MeasurementComplexItem/MeasurementComplexItem.cc

@@ -2,6 +2,7 @@
 
 #include "CircularGenerator.h"
 #include "LinearGenerator.h"
+#include "NemoInterface.h"
 #include "RoutingThread.h"
 #include "geometry/GenericCircle.h"
 #include "geometry/MeasurementArea.h"
@@ -128,6 +129,10 @@ MeasurementComplexItem::MeasurementComplexItem(
 
   resetGenerators();
   startEditing();
+
+  // connect to nemo interface
+  connect(pNemoInterface, &NemoInterface::progressChanged, this,
+          &MeasurementComplexItem::_onNewProgress);
 }
 
 MeasurementComplexItem::~MeasurementComplexItem() {}
@@ -919,6 +924,69 @@ void MeasurementComplexItem::_reverseRoute() {
   }
 }
 
+void MeasurementComplexItem::_syncTiles() {
+  auto areaArray = _pAreaData->measurementAreaArray();
+  bool clear = false;
+  if (areaArray.size() > 0) {
+
+    // create tile ptr array
+    TilePtrArray tilePtrArray;
+    auto *pMeasurementArea = areaArray[0];
+    auto pTiles = pMeasurementArea->tiles();
+    for (int i = 0; i < pTiles->count(); ++i) {
+      auto *tile = pTiles->value<MeasurementTile *>(i);
+      Q_ASSERT(tile != nullptr);
+      tilePtrArray.push_back(tile);
+    }
+
+    if (tilePtrArray.size() > 0) {
+
+      // create id array
+      IDArray idArray;
+      for (const auto *pTile : tilePtrArray) {
+        idArray.push_back(pTile->id());
+      }
+
+      // sync. necessary?
+      bool doSync = false;
+      auto contains = pNemoInterface->containsTiles(idArray);
+      for (auto &&logical : contains) {
+        if (logical == false) {
+          doSync = true;
+          break;
+        }
+      }
+
+      if (doSync) {
+        if (!pNemoInterface->empty()) {
+          (void)pNemoInterface->clearTiles();
+        }
+        (void)pNemoInterface->addTiles(tilePtrArray);
+        return;
+      }
+    } else {
+      clear = true;
+    }
+  } else {
+    clear = true;
+  }
+
+  if (clear) {
+    if (!pNemoInterface->empty()) {
+      (void)pNemoInterface->clearTiles();
+    }
+  }
+}
+
+void MeasurementComplexItem::_onNewProgress(const ProgressArray &array) {
+  auto areaArray = this->_pAreaData->measurementAreaArray();
+  if (areaArray.size() > 0) {
+    for (auto &area : areaArray) {
+      area->updateProgress(array);
+    }
+  }
+}
+
 ComplexMissionItem::ReadyForSaveState
 MeasurementComplexItem::readyForSaveState() const {
   if (idle()) {
@@ -1145,6 +1213,10 @@ bool MeasurementComplexItem::stopEditing(bool doUpdate) {
       _updateRoute();
     }
 
+    if (correct && isDifferent) {
+      _syncTiles();
+    }
+
     return updated;
   }
   return false;
@@ -1153,6 +1225,7 @@ bool MeasurementComplexItem::stopEditing(bool doUpdate) {
 void MeasurementComplexItem::abortEditing() {
   if (editing()) {
     _setAreaData(_pAreaData);
+    _syncTiles();
     _setState(STATE::IDLE);
   }
 }

src/MeasurementComplexItem/MeasurementComplexItem.h

@@ -10,6 +10,7 @@
 #include "SettingsFact.h"
 
 #include "AreaData.h"
+#include "ProgressArray.h"
 
 class RoutingThread;
 class RoutingResult;
@@ -238,6 +239,8 @@ private slots:
   void _updateRoute();
   void _changeVariantIndex();
   void _reverseRoute();
+  void _syncTiles();
+  void _onNewProgress(const ProgressArray &array);
 
 private:
   bool _setGenerator(PtrGenerator newG);

src/MeasurementComplexItem/NemoInterface.h

@@ -2,7 +2,9 @@
 
 #include <QGeoCoordinate>
 #include <QObject>
+#include <QVariant>
 
+#include <future>
 #include <memory>
 
 #include "IDArray.h"
@@ -28,10 +30,10 @@ public:
 
   enum class STATUS {
     NOT_CONNECTED,
+    SYNC,
     READY,
     WEBSOCKET_DETECTED,
     TIMEOUT,
-    INVALID_HEARTBEAT
   };
   Q_ENUM(STATUS)
 
@@ -49,9 +51,28 @@ public:
   Q_INVOKABLE void stop();
 
   // Tile editing.
-  void addTiles(const TileArray &tileArray);
-  void removeTiles(const IDArray &idArray);
-  void clearTiles();
+  //!
+  //! \brief addTiles
+  //! \param tileArray
+  //! \return Returns a QVariant containing a boolean value, indicating if the
+  //! requested operation was successfull.
+  //!
+  std::shared_future<QVariant> addTiles(const TileArray &tileArray);
+  std::shared_future<QVariant> addTiles(const TilePtrArray &tileArray);
+  //!
+  //! \brief removeTiles
+  //! \param idArray
+  //! \return Returns a QVariant containing a boolean value, indicating if the
+  //! requested operation was successfull.
+  //!
+  std::shared_future<QVariant> removeTiles(const IDArray &idArray);
+  //!
+  //! \brief clearTiles
+  //! \return Returns a QVariant containing a boolean value, indicating if the
+  //! requested operation was successfull.
+  //!
+  std::shared_future<QVariant> clearTiles();
+
   TileArray getTiles(const IDArray &idArray);
   TileArray getAllTiles();
   LogicalArray containsTiles(const IDArray &idArray);

src/MeasurementComplexItem/geometry/GeoArea.cc

@@ -82,20 +82,7 @@ bool GeoArea::covers(const QGeoCoordinate &c) {
   }
 }
 
-void GeoArea::init() {
-  this->setObjectName(nameString);
-  //  connect(this, &GeoArea::pathChanged, [this] {
-  //    if (this->objectName() != "Tile") {
-  //      qDebug() << this->objectName() << " path: " << this->path() << "\n";
-  //    }
-  //  });
-  //  connect(this, &GeoArea::centerChanged, [this] {
-  //    if (this->objectName() != "Tile") {
-  //      qDebug() << this->objectName() << " center: " << this->center() <<
-  //      "\n";
-  //    }
-  //  });
-}
+void GeoArea::init() { this->setObjectName(nameString); }
 
 void GeoArea::setErrorString(const QString &str) {
   this->_errorString = str;

src/MeasurementComplexItem/geometry/MeasurementArea.cc

@@ -33,23 +33,6 @@ namespace {
 const char *tileArrayKey = "TileArray";
 } // namespace
 
-TileData::TileData() {}
-TileData::~TileData() {}
-
-TileData &TileData::operator=(const TileData &other) { return *this; }
-bool TileData::operator==(const TileData &other) const { return false; }
-bool TileData::operator!=(const TileData &other) const {
-  return operator==(other);
-}
-
-void TileData::saveToJson(QJsonObject &json) {}
-bool TileData::loadFromJson(const QJsonObject &json, QString &errorString) {
-  return false;
-}
-
-void TileData::clear() {}
-std::size_t TileData::size() const { return 0; }
-
 const char *MeasurementArea::settingsGroup = "MeasurementArea";
 const char *tileHeightKey = "TileHeight";
 const char *tileWidthName = "TileWidth";
@@ -107,7 +90,7 @@ MeasurementArea::MeasurementArea(const MeasurementArea &other, QObject *parent)
           qobject_cast<const MeasurementTile *>(other._tiles->operator[](i))
               ->clone(_tiles.get()));
     }
-    _tileMap = other._tileMap;
+    _indexMap = other._indexMap;
     enableUpdate();
   } else {
     enableUpdate();
@@ -131,7 +114,7 @@ MeasurementArea &MeasurementArea::operator=(const MeasurementArea &other) {
           qobject_cast<const MeasurementTile *>(other._tiles->operator[](i))
               ->clone(_tiles.get()));
     }
-    _tileMap = other._tileMap;
+    _indexMap = other._indexMap;
     enableUpdate();
   } else {
     enableUpdate();
@@ -270,6 +253,41 @@ bool MeasurementArea::loadFromJson(const QJsonObject &json,
           break;
         }
       }
+
+      if (!tileError) {
+        this->_indexMap.clear();
+        for (int i = 0; i < _tiles->count(); ++i) {
+
+          auto tile = qobject_cast<MeasurementTile *>(_tiles->get(i));
+          auto it = _indexMap.find(tile->id());
+
+          // find unique id
+          if (it != _indexMap.end()) {
+            auto newId = tile->id() + 1;
+            constexpr long counterMax = 1e6;
+            unsigned long counter = 0;
+            for (; counter <= counterMax; ++counter) {
+              it = _indexMap.find(newId);
+              if (it == _indexMap.end()) {
+                break;
+              } else {
+                ++newId;
+              }
+            }
+
+            if (counter != counterMax) {
+              tile->setId(newId);
+              tile->setProgress(0.0);
+            } else {
+              qCritical() << "MeasurementArea::storeTiles(): not able to find "
+                             "unique id!";
+              continue;
+            }
+          }
+
+          _indexMap.insert(std::make_pair(tile->id(), i));
+        }
+      }
     } else {
       qCWarning(MeasurementAreaLog)
           << "Not able to load tiles. tileArrayKey missing or wrong type.";
@@ -307,15 +325,22 @@ bool MeasurementArea::isCorrect() {
 void MeasurementArea::updateProgress(const ProgressArray &array) {
   if (ready() && !_holdProgress && array.size() > 0) {
     bool anyChanges = false;
-    for (const auto &pair : array) {
-      const auto &id = pair.first;
-      const auto &progress = pair.second;
-      auto it = _tileMap.find(id);
-      if (it != _tileMap.end()) {
-        if (!qFuzzyCompare(progress, it->second->progress())) {
-          it->second->setProgress(progress);
+    long counter = 0;
+    for (const auto &lp : array) {
+      qDebug() << "MeasurementArea::updateProgress: counter = " << counter++;
+
+      auto it = _indexMap.find(lp.id());
+      if (it != _indexMap.end()) {
+        int tileIndex = it->second;
+        auto *tile = _tiles->value<MeasurementTile *>(tileIndex);
+        qDebug() << "MeasurementArea::updateProgress: progress before = "
+                 << tile->progress();
+        if (!qFuzzyCompare(lp.progress(), tile->progress())) {
+          tile->setProgress(lp.progress());
           anyChanges = true;
         }
+        qDebug() << "MeasurementArea::updateProgress: progress after = "
+                 << tile->progress();
       }
     }
 
@@ -330,6 +355,8 @@ void MeasurementArea::randomProgress() {
 
     std::srand(std::time(nullptr));
 
+    ProgressArray progressArray;
+
     for (int i = 0; i < _tiles->count(); ++i) {
 
       auto tile = _tiles->value<MeasurementTile *>(i);
@@ -341,10 +368,10 @@ void MeasurementArea::randomProgress() {
         p = 100;
       }
 
-      tile->setProgress(p);
+      progressArray.append(LabeledProgress(p, tile->id()));
     }
 
-    emit progressChanged();
+    updateProgress(progressArray);
   }
 }
 
@@ -421,7 +448,7 @@ void MeasurementArea::doUpdate() {
               geoTile->push_back(geoVertex);
               hashValue ^= hashFun(geoVertex);
             }
-            geoTile->setId(long(hashValue));
+            geoTile->setId(std::int64_t(hashValue));
             pData->append(geoTile);
           }
         }
@@ -452,7 +479,7 @@ void MeasurementArea::deferUpdate() {
   if (this->_state == STATE::IDLE || this->_state == STATE::DEFERED) {
     qCDebug(MeasurementAreaLog) << "defereUpdate(): defer update.";
     if (this->_state == STATE::IDLE) {
-      this->_tileMap.clear();
+      this->_indexMap.clear();
       this->_tiles->clearAndDeleteContents();
       emit tilesChanged();
       emit progressChanged();
@@ -478,20 +505,20 @@ void MeasurementArea::storeTiles() {
                                    QQmlEngine::CppOwnership);
 
     // update tileMap
-    this->_tileMap.clear();
+    this->_indexMap.clear();
     for (int i = 0; i < _tiles->count(); ++i) {
 
       auto tile = qobject_cast<MeasurementTile *>(_tiles->get(i));
-      auto it = _tileMap.find(tile->id());
+      auto it = _indexMap.find(tile->id());
 
       // find unique id
-      if (it != _tileMap.end()) {
-        long newId = tile->id() + 1;
+      if (it != _indexMap.end()) {
+        auto newId = tile->id() + 1;
         constexpr long counterMax = 1e6;
         unsigned long counter = 0;
         for (; counter <= counterMax; ++counter) {
-          it = _tileMap.find(newId);
-          if (it == _tileMap.end()) {
+          it = _indexMap.find(newId);
+          if (it == _indexMap.end()) {
             break;
           } else {
             ++newId;
@@ -508,7 +535,7 @@ void MeasurementArea::storeTiles() {
         }
       }
 
-      _tileMap.insert(std::make_pair(tile->id(), tile));
+      _indexMap.insert(std::make_pair(tile->id(), i));
     }
 
     // This is expensive. Drawing tiles is expensive too.
@@ -581,18 +608,20 @@ void MeasurementArea::setHoldProgress(bool holdProgress) {
 void MeasurementArea::updateIds(const QList<TileDiff> &array) {
   for (const auto &diff : array) {
 
-    auto it = _tileMap.find(diff.oldTile.id());
-
-    if (it != _tileMap.end() &&
-        diff.oldTile.coordinateList() == it->second->coordinateList()) {
-      // Change id and update _tileMap.
-      const auto newId = diff.newTile.id();
-      auto tile = it->second;
-      tile->setId(newId);
-      _tileMap.erase(it);
-      auto ret = _tileMap.insert(std::make_pair(newId, tile));
-      Q_ASSERT(ret.second == true /*insert success?*/);
-      Q_UNUSED(ret);
+    auto it = _indexMap.find(diff.oldTile.id());
+
+    if (it != _indexMap.end()) {
+      int tileIndex = it->second;
+      auto *tile = _tiles->value<MeasurementTile *>(tileIndex);
+      if (diff.oldTile.coordinateList() == tile->coordinateList()) {
+        // Change id and update _tileMap.
+        const auto newId = diff.newTile.id();
+        tile->setId(newId);
+        _indexMap.erase(it);
+        auto ret = _indexMap.insert(std::make_pair(newId, tileIndex));
+        Q_ASSERT(ret.second == true /*insert success?*/);
+        Q_UNUSED(ret);
+      }
     }
   }
 }
@@ -673,7 +702,7 @@ bool getTiles(const FPolygon &area, Length tileHeight, Length tileWidth,
                                          boost_tiles))
         continue;
 
-      for (FPolygon t : boost_tiles) {
+      for (FPolygon &t : boost_tiles) {
         if (bg::area(t) > minTileArea.value()) {
           // Transform boost_tile to world coordinate system.
           FPolygon rotated_tile;

src/MeasurementComplexItem/geometry/MeasurementArea.h

@@ -54,10 +54,10 @@ public:
   // Overrides from GeoArea
   QString mapVisualQML(void) const override;
   QString editorQML(void) const override;
-  MeasurementArea *clone(QObject *parent = nullptr) const;
+  MeasurementArea *clone(QObject *parent = nullptr) const override;
   bool saveToJson(QJsonObject &json) override;
   bool loadFromJson(const QJsonObject &json, QString &errorString) override;
-  Q_INVOKABLE virtual bool isCorrect();
+  Q_INVOKABLE virtual bool isCorrect() override;
 
   // Property getters.
   Fact *tileHeight();
@@ -122,7 +122,7 @@ private:
 
   // Tile stuff.
   TilePtr _tiles;
-  std::map<long /*id*/, MeasurementTile *> _tileMap;
+  std::map<std::int64_t /*id*/, int> _indexMap;
   bool _holdProgress;
   QTimer _timer;
   STATE _state;

src/MeasurementComplexItem/geometry/ProgressArray.h

@@ -4,7 +4,10 @@
 #include <QVector>
 #include <tuple>
 
-typedef std::pair<long /*id*/, double /*progress*/> TaggedProgress;
-typedef QVector<TaggedProgress> ProgressArray;
+#include "comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h"
+
+typedef ros_bridge::messages::nemo_msgs::labeled_progress::LabeledProgress
+    LabeledProgress;
+typedef QVector<LabeledProgress> ProgressArray;
 
 #endif // PROGRESSARRAY_H

src/MeasurementComplexItem/nemo_interface/CommandDispatcher.h

-#ifndef COMMANDDISPATCHER_H
-#define COMMANDDISPATCHER_H
-
-#include <QThread>
-#include <QVariant>
-
-#include "Task.h"
-
-namespace nemo_interface {
-
-class CommandDispatcher : public QThread {
-public:
-  CommandDispatcher();
-
-  bool interruptionPoint(); // thread safe
-
-  std::future<QVariant> dispatch(const Task &c);     // thread safe
-  std::future<QVariant> dispatchNext(const Task &c); // thread safe