nemointerface improved

qgroundcontrol.pro

@@ -29,20 +29,16 @@ DebugBuild {
     DESTDIR  = $${OUT_PWD}/debug
     DEFINES += DEBUG
     #DEFINES += SNAKE_SHOW_TIME
-    #DEFINES += DEBUG_SRTL
     #DEFINES += SNAKE_DEBUG
     DEFINES += SNAKE_SHOW_TIME
-    DEFINES += SHOW_CIRCULAR_SURVEY_TIME
     DEFINES += DEBUG_CIRCULAR_SURVEY
-    #DEFINES += ROS_BRIDGE_DEBUG
+    DEFINES += ROS_BRIDGE_DEBUG
 }
 else {
     DESTDIR  = $${OUT_PWD}/release
     #DEFINES += ROS_BRIDGE_DEBUG
-    DEFINES += SHOW_CIRCULAR_SURVEY_TIME
     DEFINES += SNAKE_SHOW_TIME
     #DEFINES += SNAKE_DEBUG
-    #DEFINES += DEBUG_SRTL
     DEFINES += NDEBUG
 }
 

src/Wima/Geometry/WimaArea.cc

@@ -70,7 +70,7 @@ WimaArea::WimaArea(const WimaArea &other, QObject *parent)
  */
 WimaArea &WimaArea::operator=(const WimaArea &other) {
   QGCMapPolygon::operator=(other);
-  this->_maxAltitude = other.maxAltitude();
+  this->setMaxAltitude(other._maxAltitude);
   this->setPath(other.path());
 
   return *this;

src/Wima/Geometry/WimaMeasurementArea.cc

@@ -58,8 +58,6 @@ const char *WimaMeasurementArea::settingsGroup = "MeasurementArea";
 const char *WimaMeasurementArea::tileHeightName = "TileHeight";
 const char *WimaMeasurementArea::tileWidthName = "TileWidth";
 const char *WimaMeasurementArea::minTileAreaName = "MinTileArea";
-const char *WimaMeasurementArea::transectDistanceName = "TransectDistance";
-const char *WimaMeasurementArea::minTransectLengthName = "MinTransectLength";
 const char *WimaMeasurementArea::showTilesName = "ShowTiles";
 const char *WimaMeasurementArea::WimaMeasurementAreaName = "Measurement Area";
 
@@ -76,12 +74,6 @@ WimaMeasurementArea::WimaMeasurementArea(QObject *parent)
                               this /* QObject parent */)),
       _minTileArea(SettingsFact(settingsGroup, _metaDataMap[minTileAreaName],
                                 this /* QObject parent */)),
-      _transectDistance(SettingsFact(settingsGroup,
-                                     _metaDataMap[transectDistanceName],
-                                     this /* QObject parent */)),
-      _minTransectLength(SettingsFact(settingsGroup,
-                                      _metaDataMap[minTransectLengthName],
-                                      this /* QObject parent */)),
       _showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName],
                               this /* QObject parent */)),
       _state(STATE::IDLE) {
@@ -100,12 +92,6 @@ WimaMeasurementArea::WimaMeasurementArea(const WimaMeasurementArea &other,
                               this /* QObject parent */)),
       _minTileArea(SettingsFact(settingsGroup, _metaDataMap[minTileAreaName],
                                 this /* QObject parent */)),
-      _transectDistance(SettingsFact(settingsGroup,
-                                     _metaDataMap[transectDistanceName],
-                                     this /* QObject parent */)),
-      _minTransectLength(SettingsFact(settingsGroup,
-                                      _metaDataMap[minTransectLengthName],
-                                      this /* QObject parent */)),
       _showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName],
                               this /* QObject parent */)),
       _state(STATE::IDLE) {
@@ -120,7 +106,6 @@ WimaMeasurementArea::WimaMeasurementArea(const WimaMeasurementArea &other,
 WimaMeasurementArea &WimaMeasurementArea::
 operator=(const WimaMeasurementArea &other) {
   WimaArea::operator=(other);
-
   return *this;
 }
 
@@ -140,10 +125,6 @@ Fact *WimaMeasurementArea::tileWidth() { return &_tileWidth; }
 
 Fact *WimaMeasurementArea::minTileArea() { return &_minTileArea; }
 
-Fact *WimaMeasurementArea::transectDistance() { return &_transectDistance; }
-
-Fact *WimaMeasurementArea::minTransectLength() { return &_minTransectLength; }
-
 Fact *WimaMeasurementArea::showTiles() { return &_showTiles; }
 
 QmlObjectListModel *WimaMeasurementArea::tiles() {
@@ -179,8 +160,6 @@ void WimaMeasurementArea::saveToJson(QJsonObject &json) {
   json[tileHeightName] = _tileHeight.rawValue().toDouble();
   json[tileWidthName] = _tileWidth.rawValue().toDouble();
   json[minTileAreaName] = _minTileArea.rawValue().toDouble();
-  json[transectDistanceName] = _transectDistance.rawValue().toDouble();
-  json[minTransectLengthName] = _minTransectLength.rawValue().toDouble();
   json[showTilesName] = _showTiles.rawValue().toBool();
   json[areaTypeName] = WimaMeasurementAreaName;
 }
@@ -211,22 +190,6 @@ bool WimaMeasurementArea::loadFromJson(const QJsonObject &json,
       retVal = false;
     }
 
-    if (json.contains(transectDistanceName) &&
-        json[transectDistanceName].isDouble()) {
-      _transectDistance.setRawValue(json[transectDistanceName].toDouble());
-    } else {
-      errorString.append(tr("Could not load transect distance!\n"));
-      retVal = false;
-    }
-
-    if (json.contains(minTransectLengthName) &&
-        json[minTransectLengthName].isDouble()) {
-      _minTransectLength.setRawValue(json[minTransectLengthName].toDouble());
-    } else {
-      errorString.append(tr("Could not load minimal transect length!\n"));
-      retVal = false;
-    }
-
     if (json.contains(showTilesName) && json[showTilesName].isBool()) {
       _showTiles.setRawValue(json[showTilesName].toBool());
     } else {

src/Wima/Geometry/WimaMeasurementArea.h

@@ -44,8 +44,6 @@ public:
   Q_PROPERTY(Fact *tileHeight READ tileHeight CONSTANT)
   Q_PROPERTY(Fact *tileWidth READ tileWidth CONSTANT)
   Q_PROPERTY(Fact *minTileArea READ minTileArea CONSTANT)
-  Q_PROPERTY(Fact *transectDistance READ transectDistance CONSTANT)
-  Q_PROPERTY(Fact *minTransectLength READ minTransectLength CONSTANT)
   Q_PROPERTY(Fact *showTiles READ showTiles CONSTANT)
   Q_PROPERTY(QmlObjectListModel *tiles READ tiles NOTIFY tilesChanged)
   Q_PROPERTY(int maxTiles READ maxTiles NOTIFY maxTilesChanged)
@@ -55,11 +53,10 @@ public:
   QString mapVisualQML(void) const;
   QString editorQML(void) const;
 
+  // Property getters.
   Fact *tileHeight();
   Fact *tileWidth();
   Fact *minTileArea();
-  Fact *transectDistance();
-  Fact *minTransectLength();
   Fact *showTiles();
   QmlObjectListModel *tiles();
   const QVector<int> &progress() const;
@@ -83,8 +80,6 @@ public:
   static const char *tileHeightName;
   static const char *tileWidthName;
   static const char *minTileAreaName;
-  static const char *transectDistanceName;
-  static const char *minTransectLengthName;
   static const char *showTilesName;
   static const char *WimaMeasurementAreaName;
 
@@ -93,6 +88,7 @@ signals:
   void maxTilesChanged();
   void progressChanged();
   void progressAccepted();
+  void progressNotAccepted();
   void readyChanged();
 
 public slots:
@@ -114,17 +110,14 @@ private:
   SettingsFact _tileHeight;
   SettingsFact _tileWidth;
   SettingsFact _minTileArea;
-  SettingsFact _transectDistance;
-  SettingsFact _minTransectLength;
   SettingsFact _showTiles;
 
   // Tile stuff.
-  QTimer _timer;
+  mutable QTimer _timer;
   using DataPtr = std::shared_ptr<TileData>;
-  TileData _tileData;
-  QFutureWatcher<DataPtr> _watcher;
-
-  STATE _state;
+  mutable TileData _tileData;
+  mutable QFutureWatcher<DataPtr> _watcher;
+  mutable STATE _state;
 
   QVector<int> _progress;
 };

src/Wima/Snake/NemoInterface.cpp

@@ -55,7 +55,7 @@ public:
   bool running();
 
 private:
-  bool doTopicServiceSetup();
+  void doTopicServiceSetup();
   void loop();
   static STATUS heartbeatToStatus(
       const ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat &hb);
@@ -69,6 +69,7 @@ private:
   //! \pre this->ENUOriginMutex must be locked
   //!
   void publishENUOrigin();
+  bool setStatus(NemoInterface::STATUS s);
 
   // Data.
   SnakeTilesLocal tilesENU;
@@ -77,9 +78,9 @@ private:
   mutable std::shared_timed_mutex ENUOriginMutex;
   QNemoProgress qProgress;
   mutable std::shared_timed_mutex progressMutex;
-  NemoInterface::STATUS status_;
   TimePoint nextTimeout;
-  mutable std::shared_timed_mutex statusMutex;
+  mutable std::shared_timed_mutex timeoutMutex;
+  std::atomic<NemoInterface::STATUS> status_;
 
   // Not protected data.
   TileData tileData;
@@ -106,7 +107,7 @@ StatusMap statusMap{
         NemoInterface::STATUS::WEBSOCKET_DETECTED, "Websocket Detected")};
 
 NemoInterface::Impl::Impl(NemoInterface *p)
-    : status_(STATUS::NOT_CONNECTED), nextTimeout(TimePoint::max()),
+    : nextTimeout(TimePoint::max()), status_(STATUS::NOT_CONNECTED),
       running_(false), topicServiceSetupDone(false), parent(p) {
 
   // ROS Bridge.
@@ -158,25 +159,37 @@ void NemoInterface::Impl::setTileData(const TileData &tileData) {
     const auto *tile = qobject_cast<const SnakeTile *>(obj);
     if (tile != nullptr) {
       if (tile->coordinateList().size() > 0) {
-        this->ENUOrigin = tile->coordinateList().first();
-        const auto &origin = this->ENUOrigin;
-        this->tilesENU.polygons().clear();
-        for (int i = 0; i < tileData.tiles.count(); ++i) {
-          obj = tileData.tiles.get(i);
-          tile = qobject_cast<const SnakeTile *>(obj);
-          if (tile != nullptr) {
-            SnakeTileLocal tileENU;
-            snake::areaToEnu(origin, tile->coordinateList(), tileENU.path());
-            this->tilesENU.polygons().push_back(std::move(tileENU));
-          } else {
-            qWarning() << "NemoInterface::Impl::setTileData(): nullptr.";
-            break;
+        if (tile->coordinateList().first().isValid()) {
+          this->ENUOrigin = tile->coordinateList().first();
+          const auto &origin = this->ENUOrigin;
+          this->tilesENU.polygons().clear();
+          for (int i = 0; i < tileData.tiles.count(); ++i) {
+            obj = tileData.tiles.get(i);
+            tile = qobject_cast<const SnakeTile *>(obj);
+            if (tile != nullptr) {
+              SnakeTileLocal tileENU;
+              snake::areaToEnu(origin, tile->coordinateList(), tileENU.path());
+              this->tilesENU.polygons().push_back(std::move(tileENU));
+            } else {
+              qWarning() << "NemoInterface::Impl::setTileData(): nullptr.";
+              break;
+            }
           }
+        } else {
+          qWarning() << "NemoInterface::Impl::setTileData(): Origin invalid.";
         }
       } else {
-        qWarning() << "NemoInterface::Impl::setTileData(): nullptr.";
+        qWarning() << "NemoInterface::Impl::setTileData(): tile empty.";
       }
     }
+  } else {
+    this->tileData.clear();
+
+    std::lock(this->ENUOriginMutex, this->tilesENUMutex);
+    UniqueLock lk1(this->ENUOriginMutex, std::adopt_lock);
+    UniqueLock lk2(this->tilesENUMutex, std::adopt_lock);
+    this->ENUOrigin = QGeoCoordinate(0, 0, 0);
+    this->tilesENU = SnakeTilesLocal();
   }
 }
 
@@ -196,10 +209,7 @@ void NemoInterface::Impl::publishTileData() {
   }
 }
 
-NemoInterface::STATUS NemoInterface::Impl::status() {
-  SharedLock lk(this->statusMutex);
-  return status_;
-}
+NemoInterface::STATUS NemoInterface::Impl::status() { return status_.load(); }
 
 QVector<int> NemoInterface::Impl::progress() {
   SharedLock lk(this->progressMutex);
@@ -208,15 +218,12 @@ QVector<int> NemoInterface::Impl::progress() {
 
 bool NemoInterface::Impl::running() { return this->running_.load(); }
 
-bool NemoInterface::Impl::doTopicServiceSetup() {
+void NemoInterface::Impl::doTopicServiceSetup() {
   using namespace ros_bridge::messages;
 
   // snake tiles.
   {
     SharedLock lk(this->tilesENUMutex);
-
-    if (this->tilesENU.polygons().size() == 0)
-      return false;
     this->pRosBridge->advertiseTopic(
         "/snake/tiles",
         jsk_recognition_msgs::polygon_array::messageType().c_str());
@@ -225,7 +232,6 @@ bool NemoInterface::Impl::doTopicServiceSetup() {
   // snake origin.
   {
     SharedLock lk(this->ENUOriginMutex);
-
     this->pRosBridge->advertiseTopic(
         "/snake/origin", geographic_msgs::geo_point::messageType().c_str());
   }
@@ -261,21 +267,20 @@ bool NemoInterface::Impl::doTopicServiceSetup() {
       /* callback */ [this](JsonDocUPtr pDoc) {
         //        auto start = std::chrono::high_resolution_clock::now();
         nemo_msgs::heartbeat::Heartbeat heartbeatMsg;
-        UniqueLock lk(this->statusMutex);
         if (!nemo_msgs::heartbeat::fromJson(*pDoc, heartbeatMsg)) {
-          status_ = STATUS::INVALID_HEARTBEAT;
+          this->setStatus(STATUS::INVALID_HEARTBEAT);
         } else {
-          status_ = heartbeatToStatus(heartbeatMsg);
+          this->setStatus(heartbeatToStatus(heartbeatMsg));
         }
-        if (status_ == STATUS::INVALID_HEARTBEAT) {
+        if (this->status_ == STATUS::INVALID_HEARTBEAT) {
+          UniqueLock lk(this->timeoutMutex);
           this->nextTimeout = TimePoint::max();
-        } else if (status_ == STATUS::HEARTBEAT_DETECTED) {
+        } else if (this->status_ == STATUS::HEARTBEAT_DETECTED) {
+          UniqueLock lk(this->timeoutMutex);
           this->nextTimeout =
               std::chrono::high_resolution_clock::now() + timeoutInterval;
         }
-        lk.unlock();
 
-        emit this->parent->statusChanged();
         //        auto delta =
         //        std::chrono::duration_cast<std::chrono::milliseconds>(
         //            std::chrono::high_resolution_clock::now() - start);
@@ -320,8 +325,6 @@ bool NemoInterface::Impl::doTopicServiceSetup() {
         pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator());
         return pDoc;
       });
-
-  return true;
 }
 
 void NemoInterface::Impl::loop() {
@@ -329,25 +332,20 @@ void NemoInterface::Impl::loop() {
   if (this->running_) {
     if (!this->pRosBridge->isRunning()) {
       this->pRosBridge->start();
+      this->loop();
     } else if (this->pRosBridge->isRunning() && this->pRosBridge->connected() &&
                !this->topicServiceSetupDone) {
-      if (this->doTopicServiceSetup()) {
-        this->topicServiceSetupDone = true;
-        UniqueLock lk(this->statusMutex);
-        this->status_ = STATUS::WEBSOCKET_DETECTED;
-        lk.unlock();
-        emit this->parent->statusChanged();
-      }
+      this->doTopicServiceSetup();
+      this->topicServiceSetupDone = true;
+
+      this->setStatus(STATUS::WEBSOCKET_DETECTED);
     } else if (this->pRosBridge->isRunning() &&
                !this->pRosBridge->connected() && this->topicServiceSetupDone) {
       this->pRosBridge->reset();
       this->pRosBridge->start();
       this->topicServiceSetupDone = false;
 
-      UniqueLock lk(this->statusMutex);
-      this->status_ = STATUS::TIMEOUT;
-      lk.unlock();
-      emit this->parent->statusChanged();
+      this->setStatus(STATUS::TIMEOUT);
     }
   } else if (this->pRosBridge->isRunning()) {
     this->pRosBridge->reset();
@@ -356,16 +354,15 @@ void NemoInterface::Impl::loop() {
 
   // Check if heartbeat timeout occured.
   if (this->running_ && this->topicServiceSetupDone) {
-    UniqueLock lk(this->statusMutex);
+    UniqueLock lk(this->timeoutMutex);
     if (this->nextTimeout != TimePoint::max() &&
         this->nextTimeout < std::chrono::high_resolution_clock::now()) {
+      lk.unlock();
       if (this->pRosBridge->isRunning() && this->pRosBridge->connected()) {
-        this->status_ = STATUS::WEBSOCKET_DETECTED;
+        this->setStatus(STATUS::WEBSOCKET_DETECTED);
       } else {
-        this->status_ = STATUS::TIMEOUT;
+        this->setStatus(STATUS::TIMEOUT);
       }
-      lk.unlock();
-      emit this->parent->statusChanged();
     }
   }
 }
@@ -400,6 +397,16 @@ void NemoInterface::Impl::publishENUOrigin() {
   this->pRosBridge->publish(std::move(jOrigin), "/snake/origin");
 }
 
+bool NemoInterface::Impl::setStatus(NemoInterface::STATUS s) {
+  if (s != this->status_) {
+    this->status_ = s;
+    emit this->parent->statusChanged();
+    return true;
+  } else {
+    return false;
+  }
+}
+
 // ===============================================================
 // NemoInterface
 NemoInterface::NemoInterface(QObject *parent)

src/Wima/Snake/snake.cpp

@@ -1143,10 +1143,17 @@ bool shortestPathFromGraph(const Matrix<double> &graph, const size_t startIndex,
 
 } // namespace snake
 
-bool boost::geometry::model::operator==(snake::FPoint p1, snake::FPoint p2) {
+bool boost::geometry::model::operator==(snake::FPoint &p1, snake::FPoint &p2) {
   return (p1.get<0>() == p2.get<0>()) && (p1.get<1>() == p2.get<1>());
 }
 
-bool boost::geometry::model::operator!=(snake::FPoint p1, snake::FPoint p2) {
+bool boost::geometry::model::operator!=(snake::FPoint &p1, snake::FPoint &p2) {
+  return !(p1 == p2);
+}
+
+bool boost::geometry::model::operator==(snake::IPoint &p1, snake::IPoint &p2) {
+  return (p1.get<0>() == p2.get<0>()) && (p1.get<1>() == p2.get<1>());
+}
+bool boost::geometry::model::operator!=(snake::IPoint &p1, snake::IPoint &p2) {
   return !(p1 == p2);
 }

src/Wima/Snake/snake.h

@@ -258,8 +258,10 @@ namespace boost {
 namespace geometry {
 namespace model {
 
-bool operator==(snake::FPoint p1, snake::FPoint p2);
-bool operator!=(snake::FPoint p1, snake::FPoint p2);
+bool operator==(snake::FPoint &p1, snake::FPoint &p2);
+bool operator!=(snake::FPoint &p1, snake::FPoint &p2);
+bool operator==(snake::IPoint &p1, snake::IPoint &p2);
+bool operator!=(snake::IPoint &p1, snake::IPoint &p2);
 
 } // namespace model
 } // namespace geometry

src/Wima/WimaPlaner.cc

@@ -160,8 +160,6 @@ bool WimaPlaner::readyForSynchronization() {
 
 bool WimaPlaner::progressLocked() { return this->_progressLocked; }
 
-WimaPlaner *WimaPlaner::thisPointer() { return this; }
-
 void WimaPlaner::removeArea(int index) {
   if (index >= 0 && index < _visualItems.count()) {
     WimaArea *area = qobject_cast<WimaArea *>(_visualItems.removeAt(index));
@@ -239,15 +237,20 @@ bool WimaPlaner::addCorridor() {
 
 void WimaPlaner::removeAll() {
   bool changesApplied = false;
+  // Delete Pointers.
   while (_visualItems.count() > 0) {
     removeArea(0);
     changesApplied = true;
   }
+  // Reset Items.
+  _measurementArea = WimaMeasurementArea();
+  _joinedArea = WimaJoinedArea();
+  _serviceArea = WimaServiceArea();
+  _corridor = WimaCorridor();
 
+  // Remove missions items.
   _missionController->removeAll();
-
   _currentFile = "";
-
   _survey = nullptr;
 
   emit currentFileChanged();

src/Wima/WimaPlaner.h

@@ -82,7 +82,6 @@ public:
   void setCurrentPolygonIndex(int index);
   void setProgressLocked(bool l);
 
-  Q_INVOKABLE WimaPlaner *thisPointer();
   Q_INVOKABLE bool addMeasurementArea();
   /// Removes an area from _visualItems
   /// @param index Index of the area to be removed