diff --git a/src/Wima/Geometry/WimaMeasurementArea.cc b/src/Wima/Geometry/WimaMeasurementArea.cc index 0aa6b52041667c184512957d6c9936cca4805c2e..e02bef9ff177b74144be8b56eb9e5d07d94aff7f 100644 --- a/src/Wima/Geometry/WimaMeasurementArea.cc +++ b/src/Wima/Geometry/WimaMeasurementArea.cc @@ -72,7 +72,7 @@ WimaMeasurementArea::WimaMeasurementArea(QObject *parent) this /* QObject parent */)), _minTileAreaPercent(SettingsFact(settingsGroup, _metaDataMap[minTileAreaName], - this /* QObject parent */)), + this /* QObject parent */)), _showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName], this /* QObject parent */)), _state(STATE::IDLE) { @@ -91,7 +91,7 @@ WimaMeasurementArea::WimaMeasurementArea(const WimaMeasurementArea &other, this /* QObject parent */)), _minTileAreaPercent(SettingsFact(settingsGroup, _metaDataMap[minTileAreaName], - this /* QObject parent */)), + this /* QObject parent */)), _showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName], this /* QObject parent */)), _state(STATE::IDLE) { @@ -157,12 +157,12 @@ bool WimaMeasurementArea::ready() const { return this->_state == STATE::IDLE; } void WimaMeasurementArea::saveToJson(QJsonObject &json) { if (ready()) { - this->WimaArea::saveToJson(json); - json[tileHeightName] = _tileHeight.rawValue().toDouble(); - json[tileWidthName] = _tileWidth.rawValue().toDouble(); + this->WimaArea::saveToJson(json); + json[tileHeightName] = _tileHeight.rawValue().toDouble(); + json[tileWidthName] = _tileWidth.rawValue().toDouble(); json[minTileAreaName] = _minTileAreaPercent.rawValue().toDouble(); - json[showTilesName] = _showTiles.rawValue().toBool(); - json[areaTypeName] = WimaMeasurementAreaName; + json[showTilesName] = _showTiles.rawValue().toBool(); + json[areaTypeName] = WimaMeasurementAreaName; } else { qCDebug(WimaMeasurementAreaLog) << "saveToJson(): not ready for saveing."; } @@ -231,70 +231,70 @@ void WimaMeasurementArea::doUpdate() { using namespace boost::units; auto start = std::chrono::high_resolution_clock::now(); - // Check state. + if (this->_state != STATE::UPDATEING && this->_state != STATE::STOP) { - const auto height = this->_tileHeight.rawValue().toDouble() * si::meter; - const auto width = this->_tileWidth.rawValue().toDouble() * si::meter; - const auto tileArea = width * height; - const auto totalArea = this->area() * si::meter * si::meter; - const auto estNumTiles = totalArea / tileArea; + const auto height = this->_tileHeight.rawValue().toDouble() * si::meter; + const auto width = this->_tileWidth.rawValue().toDouble() * si::meter; + const auto tileArea = width * height; + const auto totalArea = this->area() * si::meter * si::meter; + const auto estNumTiles = totalArea / tileArea; // Check some conditions. if (long(std::ceil(estNumTiles.value())) <= SNAKE_MAX_TILES && - this->count() >= 3 && this->isSimplePolygon()) { + this->count() >= 3 && this->isSimplePolygon()) { setState(STATE::UPDATEING); - auto polygon = this->coordinateList(); - for (auto &v : polygon) { - v.setAltitude(0); - } - const auto minArea = + auto polygon = this->coordinateList(); + for (auto &v : polygon) { + v.setAltitude(0); + } + const auto minArea = this->_minTileAreaPercent.rawValue().toDouble() / 100 * tileArea; - auto *th = this->thread(); - auto future = QtConcurrent::run([polygon, th, height, width, minArea] { - auto start = std::chrono::high_resolution_clock::now(); - - DataPtr pData(new TileData()); - // Convert to ENU system. - QGeoCoordinate origin = polygon.first(); - FPolygon polygonENU; - areaToEnu(origin, polygon, polygonENU); - std::vector tilesENU; - BoundingBox bbox; - std::string errorString; - // Generate tiles. - if (snake::tiles(polygonENU, height, width, minArea, tilesENU, bbox, - errorString)) { - // Convert to geo system. - for (const auto &t : tilesENU) { - auto geoTile = new SnakeTile(pData.get()); - for (const auto &v : t.outer()) { - QGeoCoordinate geoVertex; - fromENU(origin, v, geoVertex); - geoTile->push_back(geoVertex); - } - pData->tiles.append(geoTile); - // Calculate center. - snake::FPoint center; - snake::polygonCenter(t, center); - QGeoCoordinate geoCenter; - fromENU(origin, center, geoCenter); - pData->tileCenterPoints.append(QVariant::fromValue(geoCenter)); + auto *th = this->thread(); + auto future = QtConcurrent::run([polygon, th, height, width, minArea] { + auto start = std::chrono::high_resolution_clock::now(); + + DataPtr pData(new TileData()); + // Convert to ENU system. + QGeoCoordinate origin = polygon.first(); + FPolygon polygonENU; + areaToEnu(origin, polygon, polygonENU); + std::vector tilesENU; + BoundingBox bbox; + std::string errorString; + // Generate tiles. + if (snake::tiles(polygonENU, height, width, minArea, tilesENU, bbox, + errorString)) { + // Convert to geo system. + for (const auto &t : tilesENU) { + auto geoTile = new SnakeTile(pData.get()); + for (const auto &v : t.outer()) { + QGeoCoordinate geoVertex; + fromENU(origin, v, geoVertex); + geoTile->push_back(geoVertex); } + pData->tiles.append(geoTile); + // Calculate center. + snake::FPoint center; + snake::polygonCenter(t, center); + QGeoCoordinate geoCenter; + fromENU(origin, center, geoCenter); + pData->tileCenterPoints.append(QVariant::fromValue(geoCenter)); } - pData->moveToThread(th); + } + pData->moveToThread(th); - qCDebug(WimaMeasurementAreaLog) - << "doUpdate(): update time: " - << std::chrono::duration_cast( - std::chrono::high_resolution_clock::now() - start) - .count() - << " ms"; + qCDebug(WimaMeasurementAreaLog) + << "doUpdate(): update time: " + << std::chrono::duration_cast( + std::chrono::high_resolution_clock::now() - start) + .count() + << " ms"; - return pData; - }); // QtConcurrent::run() + return pData; + }); // QtConcurrent::run() - this->_watcher.setFuture(future); - } + this->_watcher.setFuture(future); + } } qCDebug(WimaMeasurementAreaLog) << "doUpdate(): execution time: " diff --git a/src/Wima/Geometry/WimaMeasurementArea.cc.orig b/src/Wima/Geometry/WimaMeasurementArea.cc.orig new file mode 100644 index 0000000000000000000000000000000000000000..1eca138a65b5bd172f2b558c026f33c2bf1147fb --- /dev/null +++ b/src/Wima/Geometry/WimaMeasurementArea.cc.orig @@ -0,0 +1,428 @@ +#include "WimaMeasurementArea.h" +#include "QtConcurrentRun" +#include "SnakeTile.h" +#include "snake.h" + +#include + +#include "QGCLoggingCategory.h" + +#ifndef SNAKE_MAX_TILES +#define SNAKE_MAX_TILES 1000 +#endif + +QGC_LOGGING_CATEGORY(WimaMeasurementAreaLog, "WimaMeasurementAreaLog") + +TileData::TileData() : tiles(this) {} + +TileData::~TileData() { tiles.clearAndDeleteContents(); } + +TileData &TileData::operator=(const TileData &other) { + this->tiles.clearAndDeleteContents(); + for (std::size_t i = 0; i < std::size_t(other.tiles.count()); ++i) { + const auto *obj = other.tiles.get(i); + const auto *tile = qobject_cast(obj); + if (tile != nullptr) { + this->tiles.append(new SnakeTile(*tile, this)); + } else { + qCWarning(WimaMeasurementAreaLog) << "TileData::operator=: nullptr"; + } + } + this->tileCenterPoints = other.tileCenterPoints; + return *this; +} + +bool TileData::operator==(const TileData &other) const { + return this->tiles == other.tiles && + this->tileCenterPoints == other.tileCenterPoints; +} + +bool TileData::operator!=(const TileData &other) const { + return !this->operator==(other); +} + +void TileData::clear() { + this->tiles.clearAndDeleteContents(); + this->tileCenterPoints.clear(); +} + +size_t TileData::size() const { + if (tiles.count() == tileCenterPoints.size()) { + return tiles.count(); + } else { + return 0; + } +} + +const char *WimaMeasurementArea::settingsGroup = "MeasurementArea"; +const char *WimaMeasurementArea::tileHeightName = "TileHeight"; +const char *WimaMeasurementArea::tileWidthName = "TileWidth"; +<<<<<<< HEAD +const char *WimaMeasurementArea::minTileAreaName = "MinTileAreaPercent"; +======= +const char *WimaMeasurementArea::minTileAreaName = "MinTileArea"; +>>>>>>> 118a8e164c4fec6026709eb5bee77aa75fd3de95 +const char *WimaMeasurementArea::showTilesName = "ShowTiles"; +const char *WimaMeasurementArea::WimaMeasurementAreaName = "Measurement Area"; + +WimaMeasurementArea::WimaMeasurementArea(QObject *parent) + : WimaArea(parent), + _metaDataMap(FactMetaData::createMapFromJsonFile( + QStringLiteral(":/json/WimaMeasurementArea.SettingsGroup.json"), + this /* QObject parent */)), + _tileHeight(SettingsFact(settingsGroup, _metaDataMap[tileHeightName], + this /* QObject parent */)), + _tileWidth(SettingsFact(settingsGroup, _metaDataMap[tileWidthName], + this /* QObject parent */)), +<<<<<<< HEAD + _minTileAreaPercent(SettingsFact(settingsGroup, + _metaDataMap[minTileAreaName], + this /* QObject parent */)), +======= + _minTileArea(SettingsFact(settingsGroup, _metaDataMap[minTileAreaName], + this /* QObject parent */)), +>>>>>>> 118a8e164c4fec6026709eb5bee77aa75fd3de95 + _showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName], + this /* QObject parent */)), + _state(STATE::IDLE) { + init(); +} + +WimaMeasurementArea::WimaMeasurementArea(const WimaMeasurementArea &other, + QObject *parent) + : WimaArea(other, parent), + _metaDataMap(FactMetaData::createMapFromJsonFile( + QStringLiteral(":/json/WimaMeasurementArea.SettingsGroup.json"), + this /* QObject parent */)), + _tileHeight(SettingsFact(settingsGroup, _metaDataMap[tileHeightName], + this /* QObject parent */)), + _tileWidth(SettingsFact(settingsGroup, _metaDataMap[tileWidthName], + this /* QObject parent */)), +<<<<<<< HEAD + _minTileAreaPercent(SettingsFact(settingsGroup, + _metaDataMap[minTileAreaName], + this /* QObject parent */)), +======= + _minTileArea(SettingsFact(settingsGroup, _metaDataMap[minTileAreaName], + this /* QObject parent */)), +>>>>>>> 118a8e164c4fec6026709eb5bee77aa75fd3de95 + _showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName], + this /* QObject parent */)), + _state(STATE::IDLE) { + init(); +} + +/*! + * \overload operator=() + * + * Calls the inherited operator WimaArea::operator=(). + */ +WimaMeasurementArea &WimaMeasurementArea:: +operator=(const WimaMeasurementArea &other) { + WimaArea::operator=(other); + return *this; +} + +WimaMeasurementArea::~WimaMeasurementArea() {} + +QString WimaMeasurementArea::mapVisualQML() const { + return QStringLiteral("WimaMeasurementAreaMapVisual.qml"); +} + +QString WimaMeasurementArea::editorQML() const { + return QStringLiteral("WimaMeasurementAreaEditor.qml"); +} + +Fact *WimaMeasurementArea::tileHeight() { return &_tileHeight; } + +Fact *WimaMeasurementArea::tileWidth() { return &_tileWidth; } + +<<<<<<< HEAD +Fact *WimaMeasurementArea::minTileArea() { return &_minTileAreaPercent; } +======= +Fact *WimaMeasurementArea::minTileArea() { return &_minTileArea; } +>>>>>>> 118a8e164c4fec6026709eb5bee77aa75fd3de95 + +Fact *WimaMeasurementArea::showTiles() { return &_showTiles; } + +QmlObjectListModel *WimaMeasurementArea::tiles() { + return &this->_tileData.tiles; +} + +const QVector &WimaMeasurementArea::progress() const { + return this->_progress; +} + +QVector WimaMeasurementArea::progressQml() const { + return this->_progress; +} + +const QmlObjectListModel *WimaMeasurementArea::tiles() const { + return &this->_tileData.tiles; +} + +const QVariantList &WimaMeasurementArea::tileCenterPoints() const { + return this->_tileData.tileCenterPoints; +} + +const TileData &WimaMeasurementArea::tileData() const { + return this->_tileData; +} + +int WimaMeasurementArea::maxTiles() const { return SNAKE_MAX_TILES; } + +bool WimaMeasurementArea::ready() const { return this->_state == STATE::IDLE; } + +void WimaMeasurementArea::saveToJson(QJsonObject &json) { +<<<<<<< HEAD + if (ready()) { + this->WimaArea::saveToJson(json); + json[tileHeightName] = _tileHeight.rawValue().toDouble(); + json[tileWidthName] = _tileWidth.rawValue().toDouble(); + json[minTileAreaName] = _minTileAreaPercent.rawValue().toDouble(); + json[showTilesName] = _showTiles.rawValue().toBool(); + json[areaTypeName] = WimaMeasurementAreaName; + } else { + qCDebug(WimaMeasurementAreaLog) << "saveToJson(): not ready for saveing."; + } +======= + this->WimaArea::saveToJson(json); + json[tileHeightName] = _tileHeight.rawValue().toDouble(); + json[tileWidthName] = _tileWidth.rawValue().toDouble(); + json[minTileAreaName] = _minTileArea.rawValue().toDouble(); + json[showTilesName] = _showTiles.rawValue().toBool(); + json[areaTypeName] = WimaMeasurementAreaName; +>>>>>>> 118a8e164c4fec6026709eb5bee77aa75fd3de95 +} + +bool WimaMeasurementArea::loadFromJson(const QJsonObject &json, + QString &errorString) { + if (this->WimaArea::loadFromJson(json, errorString)) { + disableUpdate(); + bool retVal = true; + + if (!json.contains(tileHeightName) || !json[tileHeightName].isDouble()) { + errorString.append(tr("Could not load tile height!\n")); + retVal = false; + } else { + _tileHeight.setRawValue(json[tileHeightName].toDouble()); + } + + if (!json.contains(tileWidthName) || !json[tileWidthName].isDouble()) { + errorString.append(tr("Could not load tile width!\n")); + retVal = false; + } else { + _tileWidth.setRawValue(json[tileWidthName].toDouble()); + } + +<<<<<<< HEAD + if (!json.contains(minTileAreaName) || !json[minTileAreaName].isDouble()) { + errorString.append(tr("Could not load minimal tile area!\n")); + retVal = false; + } else { + _minTileAreaPercent.setRawValue(json[minTileAreaName].toDouble()); + } + + if (!json.contains(showTilesName) || !json[showTilesName].isBool()) { +======= + if (json.contains(showTilesName) && json[showTilesName].isBool()) { + _showTiles.setRawValue(json[showTilesName].toBool()); + } else { +>>>>>>> 118a8e164c4fec6026709eb5bee77aa75fd3de95 + errorString.append(tr("Could not load show tiles !\n")); + retVal = false; + } else { + _showTiles.setRawValue(json[showTilesName].toBool()); + } + + enableUpdate(); + doUpdate(); + + return retVal; + } else { + return false; + } +} + +bool WimaMeasurementArea::setProgress(const QVector &p) { + if (ready()) { + if (p.size() == this->tiles()->count() && this->_progress != p) { + this->_progress = p; + emit progressChanged(); + emit progressAccepted(); + return true; + } + } + return false; +} +//! +//! \brief WimaMeasurementArea::doUpdate +//! \pre WimaMeasurementArea::deferUpdate must be called first, don't call +//! this function directly! +void WimaMeasurementArea::doUpdate() { + using namespace snake; + using namespace boost::units; + + auto start = std::chrono::high_resolution_clock::now(); + // Check state. + if (this->_state != STATE::UPDATEING && this->_state != STATE::STOP) { + const auto height = this->_tileHeight.rawValue().toDouble() * si::meter; + const auto width = this->_tileWidth.rawValue().toDouble() * si::meter; + const auto tileArea = width * height; + const auto totalArea = this->area() * si::meter * si::meter; + const auto estNumTiles = totalArea / tileArea; + // Check some conditions. + if (long(std::ceil(estNumTiles.value())) <= SNAKE_MAX_TILES && + this->count() >= 3 && this->isSimplePolygon()) { + setState(STATE::UPDATEING); + + auto polygon = this->coordinateList(); + for (auto &v : polygon) { + v.setAltitude(0); + } + const auto minArea = + this->_minTileAreaPercent.rawValue().toDouble() / 100 * tileArea; + auto *th = this->thread(); + auto future = QtConcurrent::run([polygon, th, height, width, minArea] { + auto start = std::chrono::high_resolution_clock::now(); + + DataPtr pData(new TileData()); + // Convert to ENU system. + QGeoCoordinate origin = polygon.first(); + FPolygon polygonENU; + areaToEnu(origin, polygon, polygonENU); + std::vector tilesENU; + BoundingBox bbox; + std::string errorString; + // Generate tiles. + if (snake::tiles(polygonENU, height, width, minArea, tilesENU, bbox, + errorString)) { + // Convert to geo system. + for (const auto &t : tilesENU) { + auto geoTile = new SnakeTile(pData.get()); + for (const auto &v : t.outer()) { + QGeoCoordinate geoVertex; + fromENU(origin, v, geoVertex); + geoTile->push_back(geoVertex); + } + pData->tiles.append(geoTile); + // Calculate center. + snake::FPoint center; + snake::polygonCenter(t, center); + QGeoCoordinate geoCenter; + fromENU(origin, center, geoCenter); + pData->tileCenterPoints.append(QVariant::fromValue(geoCenter)); + } + } + pData->moveToThread(th); + + qCDebug(WimaMeasurementAreaLog) + << "doUpdate(): update time: " + << std::chrono::duration_cast( + std::chrono::high_resolution_clock::now() - start) + .count() + << " ms"; + + return pData; + }); // QtConcurrent::run() + + this->_watcher.setFuture(future); + } + } + qCDebug(WimaMeasurementAreaLog) + << "doUpdate(): execution time: " + << std::chrono::duration_cast( + std::chrono::high_resolution_clock::now() - start) + .count() + << " ms"; +} + +void WimaMeasurementArea::deferUpdate() { + if (this->_state == STATE::IDLE || this->_state == STATE::DEFERED) { + qCDebug(WimaMeasurementAreaLog) << "defereUpdate(): defer update."; + if (this->_state == STATE::IDLE) { + this->_progress.clear(); + this->_tileData.clear(); + emit this->progressChanged(); + emit this->tilesChanged(); + } + this->setState(STATE::DEFERED); + this->_timer.start(100); + } else if (this->_state == STATE::UPDATEING) { + qCDebug(WimaMeasurementAreaLog) << "defereUpdate(): restart."; + setState(STATE::RESTARTING); + } +} + +void WimaMeasurementArea::storeTiles() { + auto start = std::chrono::high_resolution_clock::now(); + + if (this->_state == STATE::UPDATEING) { + qCDebug(WimaMeasurementAreaLog) << "storeTiles(): update."; + + this->_tileData = *this->_watcher.result(); + // This is expensive. Drawing tiles is expensive too. + this->_progress = QVector(this->_tileData.tiles.count(), 0); + this->progressChanged(); + emit this->tilesChanged(); + setState(STATE::IDLE); + } else if (this->_state == STATE::RESTARTING) { + qCDebug(WimaMeasurementAreaLog) << "storeTiles(): restart."; + doUpdate(); + } else if (this->_state == STATE::STOP) { + qCDebug(WimaMeasurementAreaLog) << "storeTiles(): stop."; + } + qCDebug(WimaMeasurementAreaLog) + << "storeTiles() execution time: " + << std::chrono::duration_cast( + std::chrono::high_resolution_clock::now() - start) + .count() + << " ms"; +} + +void WimaMeasurementArea::disableUpdate() { + setState(STATE::IDLE); + this->_timer.stop(); +} + +void WimaMeasurementArea::enableUpdate() { + if (this->_state == STATE::STOP) { + setState(STATE::IDLE); + } +} + +void WimaMeasurementArea::init() { + this->setObjectName(WimaMeasurementAreaName); + connect(&this->_tileHeight, &Fact::rawValueChanged, this, + &WimaMeasurementArea::deferUpdate); + connect(&this->_tileWidth, &Fact::rawValueChanged, this, + &WimaMeasurementArea::deferUpdate); + connect(&this->_minTileAreaPercent, &Fact::rawValueChanged, this, + &WimaMeasurementArea::deferUpdate); + connect(this, &WimaArea::pathChanged, this, + &WimaMeasurementArea::deferUpdate); + this->_timer.setSingleShot(true); + connect(&this->_timer, &QTimer::timeout, this, + &WimaMeasurementArea::doUpdate); + connect(&this->_watcher, + &QFutureWatcher>::finished, this, + &WimaMeasurementArea::storeTiles); +} + +void WimaMeasurementArea::setState(WimaMeasurementArea::STATE s) { + if (this->_state != s) { + auto oldState = this->_state; + this->_state = s; + if (s == STATE::IDLE || oldState == STATE::IDLE) { + emit readyChanged(); + } + } +} + +/*! + * \class WimaMeasurementArea + * \brief Class defining the area inside which the actual drone measurements + * are performed. + * + * \sa WimaArea, WimaController, WimaPlaner + */ diff --git a/src/Wima/Geometry/WimaMeasurementArea.h b/src/Wima/Geometry/WimaMeasurementArea.h index 65fcd663fefdcb73f319300c5bc74d4703443cf6..07266032584d19d5dfcdf81560e8bc77e4152fe2 100644 --- a/src/Wima/Geometry/WimaMeasurementArea.h +++ b/src/Wima/Geometry/WimaMeasurementArea.h @@ -106,7 +106,7 @@ private: SettingsFact _showTiles; QVector _progress; - + // Tile stuff. // Tile stuff. mutable QTimer _timer; using DataPtr = std::shared_ptr; diff --git a/src/Wima/Geometry/WimaMeasurementArea.h.orig b/src/Wima/Geometry/WimaMeasurementArea.h.orig new file mode 100644 index 0000000000000000000000000000000000000000..cb2f79db7af4b9905c6ef641e3baa895fbd21061 --- /dev/null +++ b/src/Wima/Geometry/WimaMeasurementArea.h.orig @@ -0,0 +1,128 @@ +#pragma once + +#include +#include +#include + +#include "WimaArea.h" + +#include "SettingsFact.h" + +class TileData : public QObject { +public: + QmlObjectListModel tiles; + QVariantList tileCenterPoints; + + TileData(); + ~TileData(); + + TileData &operator=(const TileData &other); + bool operator==(const TileData &other) const; + bool operator!=(const TileData &other) const; + + void clear(); + std::size_t size() const; +}; + +class WimaMeasurementArea : public WimaArea { + Q_OBJECT + + enum class STATE { IDLE, DEFERED, UPDATEING, RESTARTING, STOP }; + +public: + WimaMeasurementArea(QObject *parent = nullptr); + WimaMeasurementArea(const WimaMeasurementArea &other, + QObject *parent = nullptr); + WimaMeasurementArea &operator=(const WimaMeasurementArea &other); + ~WimaMeasurementArea(); + + Q_PROPERTY(Fact *tileHeight READ tileHeight CONSTANT) + Q_PROPERTY(Fact *tileWidth READ tileWidth CONSTANT) + Q_PROPERTY(Fact *minTileArea READ minTileArea CONSTANT) + Q_PROPERTY(Fact *showTiles READ showTiles CONSTANT) + Q_PROPERTY(QmlObjectListModel *tiles READ tiles NOTIFY tilesChanged) + Q_PROPERTY(int maxTiles READ maxTiles NOTIFY maxTilesChanged) + Q_PROPERTY(QVector progress READ progressQml NOTIFY progressChanged) + + // Overrides from WimaPolygon + QString mapVisualQML(void) const; + QString editorQML(void) const; + + // Property getters. + Fact *tileHeight(); + Fact *tileWidth(); + Fact *minTileArea(); + Fact *showTiles(); + QmlObjectListModel *tiles(); + const QVector &progress() const; + QVector progressQml() const; + const QmlObjectListModel *tiles() const; + const QVariantList &tileCenterPoints() const; // List of QGeoCoordinate + const TileData &tileData() const; + int maxTiles() const; + bool ready() const; + + // Member Methodes + void saveToJson(QJsonObject &json); + bool loadFromJson(const QJsonObject &json, QString &errorString); + + // Static Variables + static const char *settingsGroup; + static const char *tileHeightName; + static const char *tileWidthName; + static const char *minTileAreaName; + static const char *showTilesName; + static const char *WimaMeasurementAreaName; + +signals: + void tilesChanged(); + void maxTilesChanged(); + void progressChanged(); + void progressAccepted(); + void progressNotAccepted(); + void readyChanged(); + +public slots: + bool setProgress(const QVector &p); + +private slots: + void doUpdate(); + void deferUpdate(); + void storeTiles(); + void disableUpdate(); + void enableUpdate(); + +private: + // Member Methodes + void init(); + void setState(STATE s); + + // Members + QMap _metaDataMap; + + SettingsFact _tileHeight; + SettingsFact _tileWidth; +<<<<<<< HEAD + SettingsFact _minTileAreaPercent; // 0..100 +======= + SettingsFact _minTileArea; +>>>>>>> 118a8e164c4fec6026709eb5bee77aa75fd3de95 + SettingsFact _showTiles; + + QVector _progress; + + // Tile stuff. + mutable QTimer _timer; + using DataPtr = std::shared_ptr; +<<<<<<< HEAD + mutable STATE _state; + mutable TileData _tileData; + mutable QFutureWatcher _watcher; +======= + mutable TileData _tileData; + mutable QFutureWatcher _watcher; + mutable STATE _state; + + QVector _progress; +>>>>>>> 118a8e164c4fec6026709eb5bee77aa75fd3de95 +}; diff --git a/src/Wima/Snake/NemoInterface.cpp b/src/Wima/Snake/NemoInterface.cpp index 41b04046d57426e39a6ae65e21e0f9e8a2af9234..95b5d753e59e4d0c008cf86bcfbf75809361fe86 100644 --- a/src/Wima/Snake/NemoInterface.cpp +++ b/src/Wima/Snake/NemoInterface.cpp @@ -163,28 +163,28 @@ void NemoInterface::Impl::setTileData(const TileData &tileData) { if (tile != nullptr) { if (tile->coordinateList().size() > 0) { 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 { + 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 { qCDebug(NemoInterfaceLog) << "Impl::setTileData(): nullptr."; - break; - } + break; } - } else { - qCDebug(NemoInterfaceLog) << "Impl::setTileData(): Origin invalid."; } } else { - qCDebug(NemoInterfaceLog) << "Impl::setTileData(): tile empty."; + qCDebug(NemoInterfaceLog) << "Impl::setTileData(): Origin invalid."; } + } else { + qCDebug(NemoInterfaceLog) << "Impl::setTileData(): tile empty."; } + } } else { this->tileData.clear(); @@ -307,7 +307,7 @@ void NemoInterface::Impl::doTopicServiceSetup() { if (geographic_msgs::geo_point::toJson(origin, jOrigin, pDoc->GetAllocator())) { lk.unlock(); - pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator()); + pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator()); } else { lk.unlock(); qCWarning(NemoInterfaceLog) @@ -330,7 +330,7 @@ void NemoInterface::Impl::doTopicServiceSetup() { if (jsk_recognition_msgs::polygon_array::toJson( this->tilesENU, jSnakeTiles, pDoc->GetAllocator())) { lk.unlock(); - pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator()); + pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator()); } else { lk.unlock(); qCWarning(NemoInterfaceLog) @@ -350,7 +350,7 @@ void NemoInterface::Impl::loop() { } else if (this->pRosBridge->isRunning() && this->pRosBridge->connected() && !this->topicServiceSetupDone) { this->doTopicServiceSetup(); - this->topicServiceSetupDone = true; + this->topicServiceSetupDone = true; this->setStatus(STATUS::WEBSOCKET_DETECTED); } else if (this->pRosBridge->isRunning() && @@ -395,7 +395,7 @@ void NemoInterface::Impl::publishTilesENU() { std::make_unique(rapidjson::kObjectType)); if (jsk_recognition_msgs::polygon_array::toJson( this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator())) { - this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles"); + this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles"); } else { qCWarning(NemoInterfaceLog) << "Impl::publishTilesENU: could not create json document."; @@ -408,7 +408,7 @@ void NemoInterface::Impl::publishENUOrigin() { std::make_unique(rapidjson::kObjectType)); if (geographic_msgs::geo_point::toJson(this->ENUOrigin, *jOrigin, jOrigin->GetAllocator())) { - this->pRosBridge->publish(std::move(jOrigin), "/snake/origin"); + this->pRosBridge->publish(std::move(jOrigin), "/snake/origin"); } else { qCWarning(NemoInterfaceLog) << "Impl::publishENUOrigin: could not create json document."; diff --git a/src/Wima/Snake/NemoInterface.cpp.orig b/src/Wima/Snake/NemoInterface.cpp.orig new file mode 100644 index 0000000000000000000000000000000000000000..8daee117f44ab2ee15856199761723a3722d5b1d --- /dev/null +++ b/src/Wima/Snake/NemoInterface.cpp.orig @@ -0,0 +1,490 @@ +#include "NemoInterface.h" +#include "SnakeTilesLocal.h" + +#include "QGCApplication.h" +#include "QGCLoggingCategory.h" +#include "QGCToolbox.h" +#include "SettingsFact.h" +#include "SettingsManager.h" +#include "WimaSettings.h" + +#include + +#include + +#include "QNemoHeartbeat.h" +#include "QNemoProgress.h" +#include "Wima/Geometry/WimaMeasurementArea.h" +#include "Wima/Snake/SnakeTile.h" +#include "Wima/Snake/snake.h" + +#include "ros_bridge/include/messages/geographic_msgs/geopoint.h" +#include "ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h" +#include "ros_bridge/include/messages/nemo_msgs/heartbeat.h" +#include "ros_bridge/include/messages/nemo_msgs/progress.h" +#include "ros_bridge/include/ros_bridge.h" +#include "ros_bridge/rapidjson/include/rapidjson/document.h" +#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h" +#include "ros_bridge/rapidjson/include/rapidjson/writer.h" + +QGC_LOGGING_CATEGORY(NemoInterfaceLog, "NemoInterfaceLog") + +#define EVENT_TIMER_INTERVAL 100 // ms +auto static timeoutInterval = std::chrono::milliseconds(3000); + +using ROSBridgePtr = std::unique_ptr; +using JsonDocUPtr = ros_bridge::com_private::JsonDocUPtr; +using UniqueLock = std::unique_lock; +using SharedLock = std::shared_lock; +using JsonDocUPtr = ros_bridge::com_private::JsonDocUPtr; + +class NemoInterface::Impl { + using TimePoint = std::chrono::time_point; + +public: + Impl(NemoInterface *p); + + void start(); + void stop(); + void setTileData(const TileData &tileData); + bool hasTileData(const TileData &tileData) const; + void setAutoPublish(bool ap); + void setHoldProgress(bool hp); + + void publishTileData(); + + NemoInterface::STATUS status(); + QVector progress(); + bool running(); + +private: + void doTopicServiceSetup(); + void loop(); + static STATUS heartbeatToStatus( + const ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat &hb); + //! + //! \brief Publishes tilesENU + //! \pre this->tilesENUMutex must be locked + //! + void publishTilesENU(); + //! + //! \brief Publishes ENUOrigin + //! \pre this->ENUOriginMutex must be locked + //! + void publishENUOrigin(); + bool setStatus(NemoInterface::STATUS s); + + // Data. + SnakeTilesLocal tilesENU; + mutable std::shared_timed_mutex tilesENUMutex; + QGeoCoordinate ENUOrigin; + mutable std::shared_timed_mutex ENUOriginMutex; + QNemoProgress qProgress; + mutable std::shared_timed_mutex progressMutex; + TimePoint nextTimeout; + mutable std::shared_timed_mutex timeoutMutex; + std::atomic status_; + + // Not protected data. + TileData tileData; + + // Internals + std::atomic_bool running_; + std::atomic_bool topicServiceSetupDone; + ROSBridgePtr pRosBridge; + QTimer loopTimer; + NemoInterface *parent; +}; + +using StatusMap = std::map; +StatusMap statusMap{ + std::make_pair( + NemoInterface::STATUS::NOT_CONNECTED, "Not Connected"), + std::make_pair( + NemoInterface::STATUS::HEARTBEAT_DETECTED, "Heartbeat Detected"), + std::make_pair( + NemoInterface::STATUS::TIMEOUT, "Timeout"), + std::make_pair( + NemoInterface::STATUS::INVALID_HEARTBEAT, "Error"), + std::make_pair( + NemoInterface::STATUS::WEBSOCKET_DETECTED, "Websocket Detected")}; + +NemoInterface::Impl::Impl(NemoInterface *p) + : nextTimeout(TimePoint::max()), status_(STATUS::NOT_CONNECTED), + running_(false), topicServiceSetupDone(false), parent(p) { + + // ROS Bridge. + WimaSettings *wimaSettings = + qgcApp()->toolbox()->settingsManager()->wimaSettings(); + auto connectionStringFact = wimaSettings->rosbridgeConnectionString(); + auto setConnectionString = [connectionStringFact, this] { + auto connectionString = connectionStringFact->rawValue().toString(); + if (ros_bridge::isValidConnectionString( + connectionString.toLocal8Bit().data())) { + this->pRosBridge.reset( + new ros_bridge::ROSBridge(connectionString.toLocal8Bit().data())); + } else { + QString defaultString("localhost:9090"); + qgcApp()->showMessage("ROS Bridge connection string invalid: " + + connectionString); + qgcApp()->showMessage("Resetting connection string to: " + defaultString); + connectionStringFact->setRawValue( + QVariant(defaultString)); // calls this function recursively + } + }; + connect(connectionStringFact, &SettingsFact::rawValueChanged, + setConnectionString); + setConnectionString(); + + // Periodic. + connect(&this->loopTimer, &QTimer::timeout, [this] { this->loop(); }); + this->loopTimer.start(EVENT_TIMER_INTERVAL); +} + +void NemoInterface::Impl::start() { + this->running_ = true; + emit this->parent->runningChanged(); +} + +void NemoInterface::Impl::stop() { + this->running_ = false; + emit this->parent->runningChanged(); +} + +void NemoInterface::Impl::setTileData(const TileData &tileData) { + this->tileData = tileData; + if (tileData.tiles.count() > 0) { + std::lock(this->ENUOriginMutex, this->tilesENUMutex); + UniqueLock lk1(this->ENUOriginMutex, std::adopt_lock); + UniqueLock lk2(this->tilesENUMutex, std::adopt_lock); + + const auto *obj = tileData.tiles.get(0); + const auto *tile = qobject_cast(obj); + if (tile != nullptr) { + if (tile->coordinateList().size() > 0) { + 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 { +<<<<<<< HEAD + qCDebug(NemoInterfaceLog) << "Impl::setTileData(): nullptr."; +======= + qWarning() << "NemoInterface::Impl::setTileData(): nullptr."; +>>>>>>> 118a8e164c4fec6026709eb5bee77aa75fd3de95 + break; + } + } + } else { +<<<<<<< HEAD + qCDebug(NemoInterfaceLog) << "Impl::setTileData(): Origin invalid."; + } + } else { + qCDebug(NemoInterfaceLog) << "Impl::setTileData(): tile empty."; +======= + qWarning() << "NemoInterface::Impl::setTileData(): Origin invalid."; + } + } else { + qWarning() << "NemoInterface::Impl::setTileData(): tile empty."; +>>>>>>> 118a8e164c4fec6026709eb5bee77aa75fd3de95 + } + } + } 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(); + } +} + +bool NemoInterface::Impl::hasTileData(const TileData &tileData) const { + return this->tileData == tileData; +} + +void NemoInterface::Impl::publishTileData() { + std::lock(this->ENUOriginMutex, this->tilesENUMutex); + UniqueLock lk1(this->ENUOriginMutex, std::adopt_lock); + UniqueLock lk2(this->tilesENUMutex, std::adopt_lock); + + if (this->tilesENU.polygons().size() > 0 && this->running_ && + this->topicServiceSetupDone) { + this->publishENUOrigin(); + this->publishTilesENU(); + } +} + +NemoInterface::STATUS NemoInterface::Impl::status() { return status_.load(); } + +QVector NemoInterface::Impl::progress() { + SharedLock lk(this->progressMutex); + return this->qProgress.progress(); +} + +bool NemoInterface::Impl::running() { return this->running_.load(); } + +void NemoInterface::Impl::doTopicServiceSetup() { + using namespace ros_bridge::messages; + + // snake tiles. + { + SharedLock lk(this->tilesENUMutex); + this->pRosBridge->advertiseTopic( + "/snake/tiles", + jsk_recognition_msgs::polygon_array::messageType().c_str()); + } + + // snake origin. + { + SharedLock lk(this->ENUOriginMutex); + this->pRosBridge->advertiseTopic( + "/snake/origin", geographic_msgs::geo_point::messageType().c_str()); + } + + // Subscribe nemo progress. + this->pRosBridge->subscribe( + "/nemo/progress", + /* callback */ [this](JsonDocUPtr pDoc) { + std::lock(this->progressMutex, this->tilesENUMutex, + this->ENUOriginMutex); + UniqueLock lk1(this->progressMutex, std::adopt_lock); + UniqueLock lk2(this->tilesENUMutex, std::adopt_lock); + UniqueLock lk3(this->ENUOriginMutex, std::adopt_lock); + + int requiredSize = this->tilesENU.polygons().size(); + auto &progressMsg = this->qProgress; + if (!nemo_msgs::progress::fromJson(*pDoc, progressMsg) || + progressMsg.progress().size() != + requiredSize) { // Some error occured. + progressMsg.progress().clear(); + qgcApp()->showMessage("Invalid progress message received."); + } + emit this->parent->progressChanged(); + + lk1.unlock(); + lk2.unlock(); + lk3.unlock(); + }); + + // Subscribe /nemo/heartbeat. + this->pRosBridge->subscribe( + "/nemo/heartbeat", + /* callback */ [this](JsonDocUPtr pDoc) { + // auto start = std::chrono::high_resolution_clock::now(); + nemo_msgs::heartbeat::Heartbeat heartbeatMsg; + if (!nemo_msgs::heartbeat::fromJson(*pDoc, heartbeatMsg)) { + this->setStatus(STATUS::INVALID_HEARTBEAT); + } else { + this->setStatus(heartbeatToStatus(heartbeatMsg)); + } + if (this->status_ == STATUS::INVALID_HEARTBEAT) { + UniqueLock lk(this->timeoutMutex); + this->nextTimeout = TimePoint::max(); + } else if (this->status_ == STATUS::HEARTBEAT_DETECTED) { + UniqueLock lk(this->timeoutMutex); + this->nextTimeout = + std::chrono::high_resolution_clock::now() + timeoutInterval; + } + + // auto delta = + // std::chrono::duration_cast( + // std::chrono::high_resolution_clock::now() - start); + // std::cout << "/nemo/heartbeat callback time: " << + // delta.count() << " ms" + // << std::endl; + }); + + // Advertise /snake/get_origin. + this->pRosBridge->advertiseService( + "/snake/get_origin", "snake_msgs/GetOrigin", + [this](JsonDocUPtr) -> JsonDocUPtr { + using namespace ros_bridge::messages; + SharedLock lk(this->ENUOriginMutex); + + JsonDocUPtr pDoc( + std::make_unique(rapidjson::kObjectType)); + auto &origin = this->ENUOrigin; + rapidjson::Value jOrigin(rapidjson::kObjectType); + lk.unlock(); + if (geographic_msgs::geo_point::toJson(origin, jOrigin, + pDoc->GetAllocator())) { + lk.unlock(); + pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator()); + } else { + lk.unlock(); + qCWarning(NemoInterfaceLog) + << "/snake/get_origin service: could not create json document."; + } + + return pDoc; + }); + + // Advertise /snake/get_tiles. + this->pRosBridge->advertiseService( + "/snake/get_tiles", "snake_msgs/GetTiles", + [this](JsonDocUPtr) -> JsonDocUPtr { + SharedLock lk(this->tilesENUMutex); + + JsonDocUPtr pDoc( + std::make_unique(rapidjson::kObjectType)); + rapidjson::Value jSnakeTiles(rapidjson::kObjectType); + + if (jsk_recognition_msgs::polygon_array::toJson( + this->tilesENU, jSnakeTiles, pDoc->GetAllocator())) { + lk.unlock(); + pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator()); + } else { + lk.unlock(); + qCWarning(NemoInterfaceLog) + << "/snake/get_tiles service: could not create json document."; + } + + return pDoc; + }); +} + +void NemoInterface::Impl::loop() { + // Check ROS Bridge status and do setup if necessary. + if (this->running_) { + if (!this->pRosBridge->isRunning()) { + this->pRosBridge->start(); + this->loop(); + } else if (this->pRosBridge->isRunning() && this->pRosBridge->connected() && + !this->topicServiceSetupDone) { + 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; + + this->setStatus(STATUS::TIMEOUT); + } + } else if (this->pRosBridge->isRunning()) { + this->pRosBridge->reset(); + this->topicServiceSetupDone = false; + } + + // Check if heartbeat timeout occured. + if (this->running_ && this->topicServiceSetupDone) { + 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->setStatus(STATUS::WEBSOCKET_DETECTED); + } else { + this->setStatus(STATUS::TIMEOUT); + } + } + } +} + +NemoInterface::STATUS NemoInterface::Impl::heartbeatToStatus( + const ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat &hb) { + if (STATUS(hb.status()) == STATUS::HEARTBEAT_DETECTED) + return STATUS::HEARTBEAT_DETECTED; + else + return STATUS::INVALID_HEARTBEAT; +} + +void NemoInterface::Impl::publishTilesENU() { + using namespace ros_bridge::messages; + JsonDocUPtr jSnakeTiles( + std::make_unique(rapidjson::kObjectType)); + if (jsk_recognition_msgs::polygon_array::toJson( + this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator())) { + this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles"); + } else { + qCWarning(NemoInterfaceLog) + << "Impl::publishTilesENU: could not create json document."; + } +} + +void NemoInterface::Impl::publishENUOrigin() { + using namespace ros_bridge::messages; + JsonDocUPtr jOrigin( + std::make_unique(rapidjson::kObjectType)); + if (geographic_msgs::geo_point::toJson(this->ENUOrigin, *jOrigin, + jOrigin->GetAllocator())) { + this->pRosBridge->publish(std::move(jOrigin), "/snake/origin"); + } else { + qCWarning(NemoInterfaceLog) + << "Impl::publishENUOrigin: could not create json document."; + } +} + +bool NemoInterface::Impl::setStatus(NemoInterface::STATUS s) { + if (s != this->status_) { + this->status_ = s; + emit this->parent->statusChanged(); + return true; + } else { + return false; + } +} + +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) + : QObject(parent), pImpl(std::make_unique(this)) {} + +NemoInterface::~NemoInterface() {} + +void NemoInterface::start() { this->pImpl->start(); } + +void NemoInterface::stop() { this->pImpl->stop(); } + +void NemoInterface::publishTileData() { this->pImpl->publishTileData(); } + +void NemoInterface::requestProgress() { + qCWarning(NemoInterfaceLog) << "requestProgress(): dummy."; +} + +void NemoInterface::setTileData(const TileData &tileData) { + this->pImpl->setTileData(tileData); +} + +bool NemoInterface::hasTileData(const TileData &tileData) const { + return this->pImpl->hasTileData(tileData); +} + +int NemoInterface::status() const { return integral(this->pImpl->status()); } + +NemoInterface::STATUS NemoInterface::statusEnum() const { + return this->pImpl->status(); +} + +QString NemoInterface::statusString() const { + return statusMap.at(this->pImpl->status()); +} + +QVector NemoInterface::progress() const { return this->pImpl->progress(); } + +QString NemoInterface::editorQml() { + return QStringLiteral("NemoInterface.qml"); +} + +bool NemoInterface::running() { return this->pImpl->running(); } diff --git a/src/Wima/WimaPlaner.cc b/src/Wima/WimaPlaner.cc index cceeb54b1a6ec53b2fcce499e1ae2bea7a818cdb..a68f97406c51502bd2885c6766b46b460f00e3c1 100644 --- a/src/Wima/WimaPlaner.cc +++ b/src/Wima/WimaPlaner.cc @@ -246,7 +246,7 @@ void WimaPlaner::removeAll() { removeArea(0); changesApplied = true; } - // Reset Items. + _measurementArea = WimaMeasurementArea(); _joinedArea = WimaJoinedArea(); _serviceArea = WimaServiceArea(); diff --git a/src/Wima/WimaPlaner.cc.orig b/src/Wima/WimaPlaner.cc.orig new file mode 100644 index 0000000000000000000000000000000000000000..51ea3eea7d0883e65b8e6c77cd3cb26f0594642b --- /dev/null +++ b/src/Wima/WimaPlaner.cc.orig @@ -0,0 +1,1035 @@ +#include "WimaPlaner.h" + +#include "MissionController.h" +#include "MissionSettingsItem.h" +#include "PlanMasterController.h" +#include "QGCApplication.h" +#include "QGCLoggingCategory.h" +#include "QGCMapPolygon.h" +#include "SimpleMissionItem.h" + +#include "Geometry/GeoUtilities.h" +#include "Geometry/PlanimetryCalculus.h" +#include "OptimisationTools.h" + +#include "CircularSurvey.h" +#include "Geometry/WimaArea.h" +#include "Geometry/WimaAreaData.h" +#include "WimaBridge.h" + +#include "StateMachine.h" +using namespace wima_planer_detail; + +#include + +QGC_LOGGING_CATEGORY(WimaPlanerLog, "WimaPlanerLog") + +class CommandRAII { + std::function f; + +public: + CommandRAII(const std::function &fun) : f(fun) {} + ~CommandRAII() { f(); } +}; + +const char *WimaPlaner::wimaFileExtension = "wima"; +const char *WimaPlaner::areaItemsName = "AreaItems"; +const char *WimaPlaner::missionItemsName = "MissionItems"; + +WimaPlaner::WimaPlaner(QObject *parent) + : QObject(parent), _masterController(nullptr), _missionController(nullptr), + _currentAreaIndex(-1), _copyMAreaToSurvey(true), _copySAreaToSurvey(true), + _corridorChanged(true), _joinedArea(this), _arrivalPathLength(0), + _returnPathLength(0), _survey(nullptr), _surveyChanged(true), + _synchronized(false), _nemoInterface(this), + _stateMachine(new StateMachine), _areasMonitored(false), + _missionControllerMonitored(false), _progressLocked(false) { + + connect(this, &WimaPlaner::currentPolygonIndexChanged, this, + &WimaPlaner::updatePolygonInteractivity); + + // Monitoring. + enableAreaMonitoring(); + // Mission controller not set at this point. Not enabling monitoring. + +#ifndef NDEBUG + // for debugging and testing purpose, remove if not needed anymore + connect(&_autoLoadTimer, &QTimer::timeout, this, + &WimaPlaner::autoLoadMission); + _autoLoadTimer.setSingleShot(true); + _autoLoadTimer.start(300); +#endif + + // NemoInterface + connect(&this->_nemoInterface, &NemoInterface::progressChanged, this, + &WimaPlaner::nemoInterfaceProgressChangedHandler); + + // StateMachine + connect(this->_stateMachine.get(), &StateMachine::upToDateChanged, this, + &WimaPlaner::needsUpdateChanged); + connect(this->_stateMachine.get(), &StateMachine::surveyReadyChanged, this, + &WimaPlaner::readyForSynchronizationChanged); + connect(this->_stateMachine.get(), &StateMachine::surveyReadyChanged, this, + &WimaPlaner::surveyReadyChanged); +} + +WimaPlaner::~WimaPlaner() {} + +PlanMasterController *WimaPlaner::masterController() { + return _masterController; +} + +MissionController *WimaPlaner::missionController() { + return _missionController; +} + +QmlObjectListModel *WimaPlaner::visualItems() { return &_visualItems; } + +int WimaPlaner::currentPolygonIndex() const { return _currentAreaIndex; } + +QString WimaPlaner::currentFile() const { return _currentFile; } + +QStringList WimaPlaner::loadNameFilters() const { + QStringList filters; + + filters << tr("Supported types (*.%1 *.%2)") + .arg(wimaFileExtension) + .arg(AppSettings::planFileExtension) + << tr("All Files (*.*)"); + return filters; +} + +QStringList WimaPlaner::saveNameFilters() const { + QStringList filters; + + filters << tr("Supported types (*.%1 *.%2)") + .arg(wimaFileExtension) + .arg(AppSettings::planFileExtension); + return filters; +} + +QString WimaPlaner::fileExtension() const { return wimaFileExtension; } + +QGeoCoordinate WimaPlaner::joinedAreaCenter() const { + return _joinedArea.center(); +} + +NemoInterface *WimaPlaner::nemoInterface() { return &_nemoInterface; } + +void WimaPlaner::setMasterController(PlanMasterController *masterC) { + if (_masterController != masterC) { + _masterController = masterC; + emit masterControllerChanged(); + } +} + +void WimaPlaner::setMissionController(MissionController *missionC) { + if (_missionController != missionC) { + disableMissionControllerMonitoring(); + _missionController = missionC; + enableMissionControllerMonitoring(); + emit missionControllerChanged(); + } +} + +void WimaPlaner::setCurrentPolygonIndex(int index) { + if (index >= 0 && index < _visualItems.count() && + index != _currentAreaIndex) { + _currentAreaIndex = index; + + emit currentPolygonIndexChanged(index); + } +} + +void WimaPlaner::setProgressLocked(bool l) { + if (this->_progressLocked != l) { + this->_progressLocked = l; + emit progressLockedChanged(); + if (!this->_progressLocked) { + if (this->_measurementArea.setProgress(this->_nemoInterface.progress())) + this->_update(); + } + } +} + +bool WimaPlaner::synchronized() { return _synchronized; } + +bool WimaPlaner::needsUpdate() { return !this->_stateMachine->upToDate(); } + +bool WimaPlaner::readyForSynchronization() { + return this->_stateMachine->surveyReady(); +} + +bool WimaPlaner::surveyReady() { return this->_stateMachine->surveyReady(); } + +<<<<<<< HEAD +bool WimaPlaner::progressLocked() { return this->_progressLocked; } + +======= +>>>>>>> 118a8e164c4fec6026709eb5bee77aa75fd3de95 +void WimaPlaner::removeArea(int index) { + if (index >= 0 && index < _visualItems.count()) { + WimaArea *area = qobject_cast(_visualItems.removeAt(index)); + + if (area == nullptr) { + qCWarning(WimaPlanerLog) + << "removeArea(): nullptr catched, internal error."; + return; + } + area->clear(); + area->borderPolygon()->clear(); + + emit visualItemsChanged(); + + if (_visualItems.count() == 0) { + // this branch is reached if all items are removed + // to guarentee proper behavior, _currentAreaIndex must be set to a + // invalid value, as on constructor init. + resetAllInteractive(); + _currentAreaIndex = -1; + return; + } + + if (_currentAreaIndex >= _visualItems.count()) { + setCurrentPolygonIndex(_visualItems.count() - 1); + } else { + updatePolygonInteractivity(_currentAreaIndex); + } + } else { + qCWarning(WimaPlanerLog) << "removeArea(): Index out of bounds!"; + } +} + +bool WimaPlaner::addMeasurementArea() { + if (!_visualItems.contains(&_measurementArea)) { + _visualItems.append(&_measurementArea); + + int newIndex = _visualItems.count() - 1; + setCurrentPolygonIndex(newIndex); + + emit visualItemsChanged(); + return true; + } else { + return false; + } +} + +bool WimaPlaner::addServiceArea() { + if (!_visualItems.contains(&_serviceArea)) { + _visualItems.append(&_serviceArea); + + int newIndex = _visualItems.count() - 1; + setCurrentPolygonIndex(newIndex); + + emit visualItemsChanged(); + return true; + } else { + return false; + } +} + +bool WimaPlaner::addCorridor() { + if (!_visualItems.contains(&_corridor)) { + _visualItems.append(&_corridor); + + int newIndex = _visualItems.count() - 1; + setCurrentPolygonIndex(newIndex); + + emit visualItemsChanged(); + return true; + } else { + return false; + } +} + +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(); + if (changesApplied) + emit visualItemsChanged(); +} + +void WimaPlaner::update() { this->_update(); } + +void WimaPlaner::_update() { + setSynchronized(false); + + switch (this->_stateMachine->state()) { + case STATE::NEEDS_INIT: { + if (this->_measurementArea.ready()) { + this->_stateMachine->updateState(EVENT::INIT_DONE); + this->_update(); + } else { + this->_stateMachine->updateState(EVENT::M_AREA_NOT_READY); + } + } break; + + case STATE::WAITING_FOR_TILE_UPDATE: { + } break; + + case STATE::NEEDS_J_AREA_UPDATE: { + // check if at least service area and measurement area are available + if (_visualItems.indexOf(&_serviceArea) == -1 || + _visualItems.indexOf(&_measurementArea) == -1) + return; + + // Check if polygons have at least three vertices + if (_serviceArea.count() < 3) { + qgcApp()->showMessage(tr("Service area has less than three vertices.")); + return; + } + + if (_measurementArea.count() < 3) { + qgcApp()->showMessage( + tr("Measurement area has less than three vertices.")); + return; + } + + // Check for simple polygons + if (!_serviceArea.isSimplePolygon()) { + qgcApp()->showMessage(tr("Service area is not a simple polygon. " + "Only simple polygons allowed.\n")); + return; + } + + if (!_corridor.isSimplePolygon() && _corridor.count() > 0) { + qgcApp()->showMessage(tr("Corridor is not a simple polygon. Only " + "simple polygons allowed.\n")); + return; + } + + if (!_measurementArea.isSimplePolygon()) { + qgcApp()->showMessage(tr("Measurement area is not a simple " + "polygon. Only simple polygons allowed.\n")); + return; + } + + if (!_serviceArea.containsCoordinate(_serviceArea.depot())) { + qgcApp()->showMessage(tr("Depot not inside service area.")); + return; + } + + // Join areas. + _joinedArea.setPath(_serviceArea.path()); + if (_corridor.count() >= 3) { + _joinedArea.join(_corridor); + } + if (!_joinedArea.join(_measurementArea)) { + qgcApp()->showMessage( + tr("Not able to join areas. Service and measurement area" + " must be overlapping, or connected through a " + "corridor.")); + return; + } + this->_stateMachine->updateState(EVENT::J_AREA_UPDATED); + this->_update(); + } break; // STATE::NEEDS_J_AREA_UPDATE + + case STATE::NEEDS_SURVEY_UPDATE: { + // Need to insert Survey? + QmlObjectListModel *missionItems = _missionController->visualItems(); + int surveyIndex = missionItems->indexOf(_survey); + // Create survey item if not yet present. + if (surveyIndex < 0) { + _missionController->insertComplexMissionItem( + _missionController->circularSurveyComplexItemName(), + _measurementArea.center(), missionItems->count()); + _survey = qobject_cast( + missionItems->get(missionItems->count() - 1)); + + if (_survey == nullptr) { + qCWarning(WimaPlanerLog) << "_survey == nullptr"; + return; + } + + // establish connections + connect(_survey, &CircularSurvey::calculatingChanged, this, + &WimaPlaner::CSCalculatingChangedHandler); + connect(_survey, &CircularSurvey::missionItemReady, this, + &WimaPlaner::CSMissionItemReadyHandler); + connect(_survey, &CircularSurvey::destroyed, this, + &WimaPlaner::CSDestroyedHandler); + } + + // update survey area + disconnect(_survey, &CircularSurvey::calculatingChanged, this, + &WimaPlaner::CSCalculatingChangedHandler); + + WimaPlanData planData; + if (!toPlanData(planData)) { + qCWarning(WimaPlanerLog) << "not able to create plan data."; + return; + } + _survey->setPlanData(planData); + + connect(_survey, &CircularSurvey::calculatingChanged, this, + &WimaPlaner::CSCalculatingChangedHandler); + + // Folloing statement just for completeness. + this->_stateMachine->updateState(EVENT::SURVEY_UPDATE_TRIGGERED); + } break; // STATE::NEEDS_SURVEY_UPDATE + + case STATE::WAITING_FOR_SURVEY_UPDATE: { + } break; + + case STATE::NEEDS_PATH_UPDATE: { + // Check if survey is present. + QmlObjectListModel *missionItems = _missionController->visualItems(); + int surveyIndex = missionItems->indexOf(_survey); + if (surveyIndex < 0) { + this->_stateMachine->updateState(EVENT::SURVEY_DESTROYED); + this->_update(); + } else { + + // Remove old arrival and return path. + int size = missionItems->count(); + for (int i = surveyIndex + 1; i < size; i++) + _missionController->removeMissionItem(surveyIndex + 1); + for (int i = surveyIndex - 1; i > 1; i--) + _missionController->removeMissionItem(i); + + // set home position to serArea center + MissionSettingsItem *settingsItem = + qobject_cast(missionItems->get(0)); + if (settingsItem == nullptr) { + qCWarning(WimaPlanerLog) << "update(): settingsItem == nullptr"; + return; + } + + // set altitudes + auto depot = _serviceArea.depot(); + depot.setAltitude(0); + settingsItem->setCoordinate(depot); + + // set takeoff position + bool setCommandNeeded = false; + if (missionItems->count() < 3) { + setCommandNeeded = true; + _missionController->insertSimpleMissionItem(depot, 1); + } + SimpleMissionItem *takeOffItem = + qobject_cast(missionItems->get(1)); + if (takeOffItem == nullptr) { + qCWarning(WimaPlanerLog) << "update(): takeOffItem == nullptr"; + return; + } + if (setCommandNeeded) + _missionController->setTakeoffCommand(*takeOffItem); + takeOffItem->setCoordinate(depot); + + if (_survey->visualTransectPoints().size() == 0) { + qCWarning(WimaPlanerLog) << "update(): survey no points"; + return; + } + + // calculate path from take off to survey + QGeoCoordinate start = depot; + QGeoCoordinate end = _survey->coordinate(); + QVector path; + if (!shortestPath(start, end, path)) { + qgcApp()->showMessage( + QString(tr("Not able to calculate path from " + "takeoff position to measurement area.")) + .toLocal8Bit() + .data()); + return; + } + _arrivalPathLength = path.size() - 1; + for (int i = 1; i < path.count() - 1; i++) { + (void)_missionController->insertSimpleMissionItem( + path[i], missionItems->count() - 1); + } + + // calculate return path + start = _survey->exitCoordinate(); + end = depot; + path.clear(); + if (!shortestPath(start, end, path)) { + qgcApp()->showMessage( + QString(tr("Not able to calculate the path from " + "measurement area to landing position.")) + .toLocal8Bit() + .data()); + return; + } + _returnPathLength = + path.size() - 1; // -1: fist item is last measurement point + for (int i = 1; i < path.count() - 1; i++) { + (void)_missionController->insertSimpleMissionItem( + path[i], missionItems->count()); + } + + // Add waypoint (rover ignores land command). + (void)_missionController->insertSimpleMissionItem(depot, + missionItems->count()); + // create land position item + (void)_missionController->insertSimpleMissionItem(depot, + missionItems->count()); + SimpleMissionItem *landItem = qobject_cast( + missionItems->get(missionItems->count() - 1)); + if (landItem == nullptr) { + qCWarning(WimaPlanerLog) << "update(): landItem == nullptr"; + return; + } + if (!_missionController->setLandCommand(*landItem)) + return; + + this->_stateMachine->updateState(EVENT::PATH_UPDATED); + } + } break; // STATE::NEEDS_PATH_UPDATE + + case STATE::UP_TO_DATE: { + } break; // STATE::UP_TO_DATE + + } // switch +} + +void WimaPlaner::CSDestroyedHandler() { + this->_stateMachine->updateState(EVENT::SURVEY_DESTROYED); +} + +void WimaPlaner::CSMissionItemReadyHandler() { + this->_stateMachine->updateState(EVENT::SURVEY_UPDATED); + this->_update(); +} + +void WimaPlaner::CSCalculatingChangedHandler() { + if (this->_survey->calculating()) { + this->_stateMachine->updateState(EVENT::SURVEY_UPDATE_TRIGGERED); + } +} + +void WimaPlaner::mAreaPathChangedHandler() { + this->_stateMachine->updateState(EVENT::M_AREA_PATH_CHANGED); +} + +void WimaPlaner::mAreaTilesChangedHandler() { + this->_nemoInterface.setTileData(this->_measurementArea.tileData()); + this->_stateMachine->updateState(EVENT::M_AREA_TILES_CHANGED); +} + +void WimaPlaner::mAreaProgressChangedHandler() { + this->_stateMachine->updateState(EVENT::M_AREA_PROGRESS_CHANGED); +} + +void WimaPlaner::mAreaProgressAcceptedHandler() { this->_update(); } + +void WimaPlaner::mAreaReadyChangedHandler() { + if (this->_measurementArea.ready()) { + this->_stateMachine->updateState(EVENT::M_AREA_READY); + } else { + this->_stateMachine->updateState(EVENT::M_AREA_NOT_READY); + } +} + +void WimaPlaner::sAreaPathChangedHandler() { + this->_stateMachine->updateState(EVENT::S_AREA_PATH_CHANGED); +} + +void WimaPlaner::corridorPathChangedHandler() { + this->_stateMachine->updateState(EVENT::CORRIDOR_PATH_CHANGED); +} + +void WimaPlaner::depotChangedHandler() { + this->_stateMachine->updateState(EVENT::DEPOT_CHANGED); +} + +void WimaPlaner::missionControllerVisualItemsChangedHandler() { + // Search for survey. + auto surveyIndex = _missionController->visualItems()->indexOf(_survey); + if (surveyIndex < 0) { + // survey not found. + this->_stateMachine->updateState(EVENT::SURVEY_DESTROYED); + } else { + this->_stateMachine->updateState(EVENT::PATH_CHANGED); + } +} + +void WimaPlaner::missionControllerWaypointPathChangedHandler() { + missionControllerVisualItemsChangedHandler(); +} + +void WimaPlaner::missionControllerNewItemsFromVehicleHandler() { + this->_stateMachine->updateState(EVENT::MISSION_ITEMS_DESTROYED); +} + +void WimaPlaner::missionControllerMissionItemCountChangedHandler() { + missionControllerVisualItemsChangedHandler(); +} + +void WimaPlaner::nemoInterfaceProgressChangedHandler() { + auto p = this->_nemoInterface.progress(); + WimaBridge::instance()->setProgress(p); + + if (!progressLocked()) { + this->_measurementArea.setProgress(p); + this->_update(); + } +} + +void WimaPlaner::saveToCurrent() { saveToFile(_currentFile); } + +void WimaPlaner::saveToFile(const QString &filename) { + if (filename.isEmpty()) { + return; + } + + QString planFilename = filename; + if (!QFileInfo(filename).fileName().contains(".")) { + planFilename += QString(".%1").arg(wimaFileExtension); + } + + QFile file(planFilename); + if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) { + qgcApp()->showMessage( + tr("Plan save error %1 : %2").arg(filename).arg(file.errorString())); + _currentFile.clear(); + emit currentFileChanged(); + } else { + FileType fileType = FileType::WimaFile; + if (planFilename.contains(QString(".%1").arg(wimaFileExtension))) { + fileType = FileType::WimaFile; + } else if (planFilename.contains( + QString(".%1").arg(AppSettings::planFileExtension))) { + fileType = FileType::PlanFile; + } else { + if (planFilename.contains(".")) { + qgcApp()->showMessage(tr("File format not supported")); + } else { + qgcApp()->showMessage(tr("File without file extension not accepted.")); + return; + } + } + + QJsonDocument saveDoc = saveToJson(fileType); + file.write(saveDoc.toJson()); + if (_currentFile != planFilename) { + _currentFile = planFilename; + emit currentFileChanged(); + } + } +} + +bool WimaPlaner::loadFromCurrent() { return loadFromFile(_currentFile); } + +bool WimaPlaner::loadFromFile(const QString &filename) { + // Remove obsolete connections. + disableAreaMonitoring(); + disableMissionControllerMonitoring(); + CommandRAII onExit([this] { + this->enableAreaMonitoring(); + this->enableMissionControllerMonitoring(); + }); + + // disconnect old survey + if (_survey != nullptr) { + disconnect(_survey, &CircularSurvey::calculatingChanged, this, + &WimaPlaner::CSCalculatingChangedHandler); + disconnect(_survey, &CircularSurvey::missionItemReady, this, + &WimaPlaner::CSMissionItemReadyHandler); + disconnect(_survey, &CircularSurvey::destroyed, this, + &WimaPlaner::CSDestroyedHandler); + } + + setSynchronized(false); + + // Precondition. + QString errorString; + QString errorMessage = + tr("Error loading Plan file (%1). %2").arg(filename).arg("%1"); + + if (filename.isEmpty()) { + return false; + } + + QFileInfo fileInfo(filename); + QFile file(filename); + + if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) { + errorString = file.errorString() + QStringLiteral(" ") + filename; + qgcApp()->showMessage(errorMessage.arg(errorString)); + return false; + } + + if (fileInfo.suffix() == wimaFileExtension) { + QJsonDocument jsonDoc; + QByteArray bytes = file.readAll(); + + if (!JsonHelper::isJsonFile(bytes, jsonDoc, errorString)) { + qgcApp()->showMessage(errorMessage.arg(errorString)); + return false; + } + + QJsonObject json = jsonDoc.object(); + // AreaItems + QJsonArray areaArray = json[areaItemsName].toArray(); + _visualItems.clear(); + + int validAreaCounter = 0; + for (int i = 0; i < areaArray.size() && validAreaCounter < 3; i++) { + QJsonObject jsonArea = areaArray[i].toObject(); + + if (jsonArea.contains(WimaArea::areaTypeName) && + jsonArea[WimaArea::areaTypeName].isString()) { + if (jsonArea[WimaArea::areaTypeName] == + WimaMeasurementArea::WimaMeasurementAreaName) { + bool success = _measurementArea.loadFromJson(jsonArea, errorString); + + if (!success) { + qgcApp()->showMessage(errorMessage.arg(errorString)); + return false; + } + + validAreaCounter++; + _visualItems.append(&_measurementArea); + emit visualItemsChanged(); + } else if (jsonArea[WimaArea::areaTypeName] == + WimaServiceArea::wimaServiceAreaName) { + bool success = _serviceArea.loadFromJson(jsonArea, errorString); + + if (!success) { + qgcApp()->showMessage(errorMessage.arg(errorString)); + return false; + } + + validAreaCounter++; + _visualItems.append(&_serviceArea); + emit visualItemsChanged(); + } else if (jsonArea[WimaArea::areaTypeName] == + WimaCorridor::WimaCorridorName) { + bool success = _corridor.loadFromJson(jsonArea, errorString); + + if (!success) { + qgcApp()->showMessage(errorMessage.arg(errorString)); + return false; + } + + validAreaCounter++; + _visualItems.append(&_corridor); + emit visualItemsChanged(); + } else { + errorString += + QString(tr("%s not supported.\n").arg(WimaArea::areaTypeName)); + qgcApp()->showMessage(errorMessage.arg(errorString)); + return false; + } + } else { + errorString += QString(tr("Invalid or non existing entry for %s.\n") + .arg(WimaArea::areaTypeName)); + return false; + } + } + + _currentFile.sprintf("%s/%s.%s", fileInfo.path().toLocal8Bit().data(), + fileInfo.completeBaseName().toLocal8Bit().data(), + wimaFileExtension); + + emit currentFileChanged(); + + QJsonObject missionObject = json[missionItemsName].toObject(); + + QJsonDocument missionJsonDoc = QJsonDocument(missionObject); + // create temporary file with missionItems + QFile temporaryFile; + QString cropedFileName = filename.section("/", 0, -2); + QString temporaryFileName; + for (int i = 0;; i++) { + temporaryFileName = + cropedFileName + + QString("/temp%1.%2").arg(i).arg(AppSettings::planFileExtension); + + if (!QFile::exists(temporaryFileName)) { + temporaryFile.setFileName(temporaryFileName); + if (temporaryFile.open(QIODevice::WriteOnly | QIODevice::Text)) { + break; + } + } + + if (i > 1000) { + qCWarning(WimaPlanerLog) + << "loadFromFile(): not able to create temporary file."; + return false; + } + } + + temporaryFile.write(missionJsonDoc.toJson()); + temporaryFile.close(); + + // load from temporary file + _masterController->loadFromFile(temporaryFileName); + QmlObjectListModel *missionItems = _missionController->visualItems(); + _survey = nullptr; + for (int i = 0; i < missionItems->count(); i++) { + _survey = missionItems->value(i); + if (_survey != nullptr) { + connect(_survey, &CircularSurvey::calculatingChanged, this, + &WimaPlaner::CSCalculatingChangedHandler); + connect(_survey, &CircularSurvey::missionItemReady, this, + &WimaPlaner::CSMissionItemReadyHandler); + connect(_survey, &CircularSurvey::destroyed, this, + &WimaPlaner::CSDestroyedHandler); + break; + } + } + + // remove temporary file + if (!temporaryFile.remove()) { + qCWarning(WimaPlanerLog) + << "WimaPlaner::loadFromFile(): not able to remove " + "temporary file."; + } + return true; + } else { + errorString += QString(tr("File extension not supported.\n")); + qgcApp()->showMessage(errorMessage.arg(errorString)); + return false; + } +} + +void WimaPlaner::updatePolygonInteractivity(int index) { + if (index >= 0 && index < _visualItems.count()) { + resetAllInteractive(); + WimaArea *interactivePoly = + qobject_cast(_visualItems.get(index)); + if (interactivePoly != nullptr) + interactivePoly->setWimaAreaInteractive(true); + } +} + +void WimaPlaner::synchronize() { + if (readyForSynchronization()) { + WimaPlanData planData; + if (toPlanData(planData)) { + WimaBridge::instance()->setPlanData(planData); + setSynchronized(true); + } else { + qCWarning(WimaPlanerLog) << "error creating plan data."; + } + } +} + +bool WimaPlaner::shortestPath(const QGeoCoordinate &start, + const QGeoCoordinate &destination, + QVector &path) { + using namespace GeoUtilities; + using namespace PolygonCalculus; + QPolygonF polygon2D; + toCartesianList(_joinedArea.coordinateList(), /*origin*/ start, polygon2D); + QPointF start2D(0, 0); + QPointF end2D; + QVector path2D; + toCartesian(destination, start, end2D); + bool retVal = + PolygonCalculus::shortestPath(polygon2D, start2D, end2D, path2D); + toGeoList(path2D, /*origin*/ start, path); + + return retVal; +} + +void WimaPlaner::setSynchronized(bool s) { + if (this->_synchronized != s) { + this->_synchronized = s; + emit this->synchronizedChanged(); + } +} + +void WimaPlaner::enableAreaMonitoring() { + if (!areasMonitored()) { + connect(&this->_measurementArea, &WimaArea::pathChanged, this, + &WimaPlaner::mAreaPathChangedHandler); + connect(&this->_measurementArea, &WimaMeasurementArea::tilesChanged, this, + &WimaPlaner::mAreaTilesChangedHandler); + connect(&this->_measurementArea, &WimaMeasurementArea::progressChanged, + this, &WimaPlaner::mAreaProgressChangedHandler); + connect(&this->_measurementArea, &WimaMeasurementArea::progressAccepted, + this, &WimaPlaner::mAreaProgressAcceptedHandler); + connect(&this->_measurementArea, &WimaMeasurementArea::readyChanged, this, + &WimaPlaner::mAreaReadyChangedHandler); + connect(&this->_serviceArea, &WimaArea::pathChanged, this, + &WimaPlaner::sAreaPathChangedHandler); + connect(&this->_serviceArea, &WimaServiceArea::depotChanged, this, + &WimaPlaner::depotChangedHandler); + connect(&this->_corridor, &WimaArea::pathChanged, this, + &WimaPlaner::corridorPathChangedHandler); + this->_areasMonitored = true; + } +} + +void WimaPlaner::disableAreaMonitoring() { + if (areasMonitored()) { + disconnect(&this->_measurementArea, &WimaArea::pathChanged, this, + &WimaPlaner::mAreaPathChangedHandler); + disconnect(&this->_measurementArea, &WimaMeasurementArea::tilesChanged, + this, &WimaPlaner::mAreaTilesChangedHandler); + disconnect(&this->_measurementArea, &WimaMeasurementArea::progressChanged, + this, &WimaPlaner::mAreaProgressChangedHandler); + disconnect(&this->_measurementArea, &WimaMeasurementArea::progressAccepted, + this, &WimaPlaner::mAreaProgressAcceptedHandler); + disconnect(&this->_measurementArea, &WimaMeasurementArea::readyChanged, + this, &WimaPlaner::mAreaReadyChangedHandler); + disconnect(&this->_serviceArea, &WimaArea::pathChanged, this, + &WimaPlaner::sAreaPathChangedHandler); + disconnect(&this->_serviceArea, &WimaServiceArea::depotChanged, this, + &WimaPlaner::depotChangedHandler); + disconnect(&this->_corridor, &WimaArea::pathChanged, this, + &WimaPlaner::corridorPathChangedHandler); + this->_areasMonitored = false; + } +} + +void WimaPlaner::enableMissionControllerMonitoring() { + if (!missionControllerMonitored() && this->missionController() != nullptr) { + connect(this->missionController(), &MissionController::visualItemsChanged, + this, &WimaPlaner::missionControllerVisualItemsChangedHandler); + connect(this->missionController(), &MissionController::waypointPathChanged, + this, &WimaPlaner::missionControllerWaypointPathChangedHandler); + connect(this->missionController(), &MissionController::newItemsFromVehicle, + this, &WimaPlaner::missionControllerNewItemsFromVehicleHandler); + connect(this->missionController(), + &MissionController::missionItemCountChanged, this, + &WimaPlaner::missionControllerMissionItemCountChangedHandler); + this->_missionControllerMonitored = true; + } +} + +void WimaPlaner::disableMissionControllerMonitoring() { + if (missionControllerMonitored() && this->missionController() != nullptr) { + disconnect(this->missionController(), + &MissionController::visualItemsChanged, this, + &WimaPlaner::missionControllerVisualItemsChangedHandler); + disconnect(this->missionController(), + &MissionController::waypointPathChanged, this, + &WimaPlaner::missionControllerWaypointPathChangedHandler); + disconnect(this->missionController(), + &MissionController::newItemsFromVehicle, this, + &WimaPlaner::missionControllerNewItemsFromVehicleHandler); + disconnect(this->missionController(), + &MissionController::missionItemCountChanged, this, + &WimaPlaner::missionControllerMissionItemCountChangedHandler); + this->_missionControllerMonitored = false; + } +} + +bool WimaPlaner::areasMonitored() { return this->_areasMonitored; } + +bool WimaPlaner::missionControllerMonitored() { + return this->_missionControllerMonitored; +} + +void WimaPlaner::resetAllInteractive() { + // Marks all areas as inactive (area.interactive == false) + int itemCount = _visualItems.count(); + if (itemCount > 0) { + for (int i = 0; i < itemCount; i++) { + WimaArea *iteratorPoly = qobject_cast(_visualItems.get(i)); + iteratorPoly->setWimaAreaInteractive(false); + } + } +} + +void WimaPlaner::setInteractive() { + updatePolygonInteractivity(_currentAreaIndex); +} + +/*! + * \fn WimaPlanData WimaPlaner::toPlanData() + * + * Returns a \c WimaPlanData object containing information about the current + * mission. The \c WimaPlanData object holds only the data which is relevant + * for the \c WimaController class. Should only be called if update() was + * successful. + * + * \sa WimaController, WimaPlanData + */ +bool WimaPlaner::toPlanData(WimaPlanData &planData) { + + // store areas + planData.set(WimaMeasurementAreaData(_measurementArea)); + planData.set(WimaServiceAreaData(_serviceArea)); + planData.set(WimaCorridorData(_corridor)); + planData.set(WimaJoinedAreaData(_joinedArea)); + return true; +} + +#ifndef NDEBUG +void WimaPlaner::autoLoadMission() { + loadFromFile("/home/valentin/Desktop/drones/qgroundcontrol/Paths/" + "KlingenbachTest.wima"); + synchronize(); +} +#endif + +QJsonDocument WimaPlaner::saveToJson(FileType fileType) { + /// This function save all areas (of WimaPlaner) and all mission items (of + /// MissionController) to a QJsonDocument + /// @param fileType is either WimaFile or PlanFile (enum), if fileType == + /// PlanFile only mission items are stored + QJsonObject json; + + if (fileType == FileType::WimaFile) { + QJsonArray jsonArray; + + for (int i = 0; i < _visualItems.count(); i++) { + QJsonObject json; + + WimaArea *area = qobject_cast(_visualItems.get(i)); + + if (area == nullptr) { + qCWarning(WimaPlanerLog) << "saveing, area == nullptr!"; + return QJsonDocument(); + } + + // check the type of area, create and append the JsonObject to the + // JsonArray once determined + WimaMeasurementArea *opArea = qobject_cast(area); + if (opArea != nullptr) { + opArea->saveToJson(json); + jsonArray.append(json); + continue; + } + + WimaServiceArea *serArea = qobject_cast(area); + if (serArea != nullptr) { + serArea->saveToJson(json); + jsonArray.append(json); + continue; + } + + WimaCorridor *corridor = qobject_cast(area); + if (corridor != nullptr) { + corridor->saveToJson(json); + jsonArray.append(json); + continue; + } + + // if non of the obove branches was trigger, type must be WimaArea + area->saveToJson(json); + jsonArray.append(json); + } + + json[areaItemsName] = jsonArray; + json[missionItemsName] = _masterController->saveToJson().object(); + + return QJsonDocument(json); + } else if (fileType == FileType::PlanFile) { + return _masterController->saveToJson(); + } + + return QJsonDocument(json); +}