From 0397d6078c9dd20fd0da328a8ec52b8fe384eb33 Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Thu, 15 Oct 2020 16:23:05 +0200 Subject: [PATCH] nemointerface improved --- qgroundcontrol.pro | 6 +- src/Wima/Geometry/WimaArea.cc | 2 +- src/Wima/Geometry/WimaMeasurementArea.cc | 37 -------- src/Wima/Geometry/WimaMeasurementArea.h | 19 ++-- src/Wima/Snake/NemoInterface.cpp | 111 ++++++++++++----------- src/Wima/Snake/snake.cpp | 11 ++- src/Wima/Snake/snake.h | 6 +- src/Wima/WimaPlaner.cc | 11 ++- src/Wima/WimaPlaner.h | 1 - 9 files changed, 87 insertions(+), 117 deletions(-) diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index b06694ffb..8a40bde80 100644 --- a/qgroundcontrol.pro +++ b/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 } diff --git a/src/Wima/Geometry/WimaArea.cc b/src/Wima/Geometry/WimaArea.cc index 1eca5b37b..fcca7d2e4 100644 --- a/src/Wima/Geometry/WimaArea.cc +++ b/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; diff --git a/src/Wima/Geometry/WimaMeasurementArea.cc b/src/Wima/Geometry/WimaMeasurementArea.cc index 6837b7542..da0727f6f 100644 --- a/src/Wima/Geometry/WimaMeasurementArea.cc +++ b/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 { diff --git a/src/Wima/Geometry/WimaMeasurementArea.h b/src/Wima/Geometry/WimaMeasurementArea.h index 9909090db..374269aa7 100644 --- a/src/Wima/Geometry/WimaMeasurementArea.h +++ b/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 &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; - QFutureWatcher _watcher; - - STATE _state; + mutable TileData _tileData; + mutable QFutureWatcher _watcher; + mutable STATE _state; QVector _progress; }; diff --git a/src/Wima/Snake/NemoInterface.cpp b/src/Wima/Snake/NemoInterface.cpp index 9b0927aee..f5187778d 100644 --- a/src/Wima/Snake/NemoInterface.cpp +++ b/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 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(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(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(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 NemoInterface::Impl::progress() { SharedLock lk(this->progressMutex); @@ -208,15 +218,12 @@ QVector 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::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) diff --git a/src/Wima/Snake/snake.cpp b/src/Wima/Snake/snake.cpp index 94f3a7283..c7517cb5c 100644 --- a/src/Wima/Snake/snake.cpp +++ b/src/Wima/Snake/snake.cpp @@ -1143,10 +1143,17 @@ bool shortestPathFromGraph(const Matrix &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); } diff --git a/src/Wima/Snake/snake.h b/src/Wima/Snake/snake.h index 1627a6744..a31287b13 100644 --- a/src/Wima/Snake/snake.h +++ b/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 diff --git a/src/Wima/WimaPlaner.cc b/src/Wima/WimaPlaner.cc index 65896d152..2c3582b31 100644 --- a/src/Wima/WimaPlaner.cc +++ b/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(_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(); diff --git a/src/Wima/WimaPlaner.h b/src/Wima/WimaPlaner.h index 103319d22..eb13a6fa3 100644 --- a/src/Wima/WimaPlaner.h +++ b/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 -- 2.22.0