diff --git a/src/MissionManager/SimpleMissionItem.h b/src/MissionManager/SimpleMissionItem.h index 11a66cf4f5eaebef6af73b1975dc51434a9e7b21..fd33ad4c341f66166dba827f99bc46882c87e6bc 100644 --- a/src/MissionManager/SimpleMissionItem.h +++ b/src/MissionManager/SimpleMissionItem.h @@ -118,7 +118,7 @@ public: QString mapVisualQML (void) const override { return QStringLiteral("SimpleItemMapVisual.qml"); } void appendMissionItems (QList& items, QObject* missionItemParent) final; void applyNewAltitude (double newAltitude) final; - void setMissionFlightStatus (MissionController::MissionFlightStatus_t& missionFlightStatus) final; + virtual void setMissionFlightStatus (MissionController::MissionFlightStatus_t& missionFlightStatus) final; ReadyForSaveState readyForSaveState (void) const final; double additionalTimeDelay (void) const final; bool exitCoordinateSameAsEntry (void) const final { return true; } diff --git a/src/MissionManager/TransectStyleComplexItem.h b/src/MissionManager/TransectStyleComplexItem.h index 0709e76637b2f88844b1a70704ff474962c5f285..949c3bf3c4156471ef1e15a67123bb53c4e44e0c 100644 --- a/src/MissionManager/TransectStyleComplexItem.h +++ b/src/MissionManager/TransectStyleComplexItem.h @@ -88,8 +88,8 @@ public: // Overrides from VisualMissionItem void save (QJsonArray& planItems) override = 0; bool specifiesCoordinate (void) const override = 0; - void appendMissionItems (QList& items, QObject* missionItemParent) final; - void applyNewAltitude (double newAltitude) final; + virtual void appendMissionItems (QList& items, QObject* missionItemParent) final; + virtual void applyNewAltitude (double newAltitude) final; bool dirty (void) const final { return _dirty; } bool isSimpleItem (void) const final { return false; } bool isStandaloneCoordinate (void) const final { return false; } @@ -100,7 +100,7 @@ public: double specifiedFlightSpeed (void) final { return std::numeric_limits::quiet_NaN(); } double specifiedGimbalYaw (void) final { return std::numeric_limits::quiet_NaN(); } double specifiedGimbalPitch (void) final { return std::numeric_limits::quiet_NaN(); } - void setMissionFlightStatus (MissionController::MissionFlightStatus_t& missionFlightStatus) final; + virtual void setMissionFlightStatus (MissionController::MissionFlightStatus_t& missionFlightStatus) final; ReadyForSaveState readyForSaveState (void) const override; QString commandDescription (void) const override { return tr("Transect"); } QString commandName (void) const override { return tr("Transect"); } diff --git a/src/Wima/CircularSurvey.cc b/src/Wima/CircularSurvey.cc index 19182eb41a9947d5b3bcfc6331822c18eeb4d87e..4626e5c56c9d04bd87dae65fbaaf2a0d0a344397 100644 --- a/src/Wima/CircularSurvey.cc +++ b/src/Wima/CircularSurvey.cc @@ -19,6 +19,8 @@ #include "CircularGenerator.h" #include "LinearGenerator.h" +// ToDo: Check what happened to _transectsDirty + QGC_LOGGING_CATEGORY(CircularSurveyLog, "CircularSurveyLog") template @@ -30,9 +32,9 @@ const char *CircularSurvey::settingsGroup = "CircularSurvey"; const char *CircularSurvey::CircularSurveyName = "CircularSurvey"; const char *CircularSurvey::variantName = "Variant"; -CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView, +CircularSurvey::CircularSurvey(PlanMasterController *masterController, bool flyView, const QString &kmlOrShpFile, QObject *parent) - : TransectStyleComplexItem(vehicle, flyView, settingsGroup, parent), + : TransectStyleComplexItem(masterController, flyView, settingsGroup, parent), _state(STATE::IDLE), _metaDataMap(FactMetaData::createMapFromJsonFile( QStringLiteral(":/json/CircularSurvey.SettingsGroup.json"), this)), @@ -52,13 +54,6 @@ CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView, &CircularSurvey::_setTransects); connect(this->_pWorker.get(), &RoutingThread::calculatingChanged, this, &CircularSurvey::calculatingChanged); - this->_transectsDirty = true; - - // Altitude - connect(&_cameraCalc, &CameraCalc::distanceToSurfaceRelativeChanged, this, - &CircularSurvey::coordinateHasRelativeAltitudeChanged); - connect(&_cameraCalc, &CameraCalc::distanceToSurfaceRelativeChanged, this, - &CircularSurvey::exitCoordinateHasRelativeAltitudeChanged); // Register Generators. auto lg = std::make_shared(this->_pAreaData); @@ -137,7 +132,7 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber, return false; } - if (!_load(complexObject, errorString)) { + if (!load(complexObject, sequenceNumber, errorString)) { _ignoreRecalc = false; return false; } @@ -146,7 +141,6 @@ bool CircularSurvey::load(const QJsonObject &complexObject, int sequenceNumber, _ignoreRecalc = false; - _recalcComplexDistance(); if (_cameraShots == 0) { // Shot count was possibly not available from plan file _recalcCameraShots(); @@ -179,68 +173,6 @@ void CircularSurvey::save(QJsonArray &planItems) { bool CircularSurvey::specifiesCoordinate() const { return true; } -void CircularSurvey::appendMissionItems(QList &items, - QObject *missionItemParent) { - if (_transectsDirty) - return; - if (_loadedMissionItems.count()) { - // We have mission items from the loaded plan, use those - _appendLoadedMissionItems(items, missionItemParent); - } else { - // Build the mission items on the fly - _buildAndAppendMissionItems(items, missionItemParent); - } -} - -void CircularSurvey::_appendLoadedMissionItems(QList &items, - QObject *missionItemParent) { - if (_transectsDirty) - return; - int seqNum = _sequenceNumber; - - for (const MissionItem *loadedMissionItem : _loadedMissionItems) { - MissionItem *item = new MissionItem(*loadedMissionItem, missionItemParent); - item->setSequenceNumber(seqNum++); - items.append(item); - } -} - -void CircularSurvey::_buildAndAppendMissionItems(QList &items, - QObject *missionItemParent) { - if (_transectsDirty || _transects.count() == 0) - return; - - MissionItem *item; - int seqNum = _sequenceNumber; - - MAV_FRAME mavFrame = - followTerrain() || !_cameraCalc.distanceToSurfaceRelative() - ? MAV_FRAME_GLOBAL - : MAV_FRAME_GLOBAL_RELATIVE_ALT; - - for (const QList &transect : - _transects) { - // bool transectEntry = true; - - for (const CoordInfo_t &transectCoordInfo : transect) { - item = new MissionItem( - seqNum++, MAV_CMD_NAV_WAYPOINT, mavFrame, - 0, // Hold time (delay for hover and capture to settle vehicle - // before image is taken) - 0.0, // No acceptance radius specified - 0.0, // Pass through waypoint - std::numeric_limits::quiet_NaN(), // Yaw unchanged - transectCoordInfo.coord.latitude(), - transectCoordInfo.coord.longitude(), - transectCoordInfo.coord.altitude(), - true, // autoContinue - false, // isCurrentItem - missionItemParent); - items.append(item); - } - } -} - bool CircularSurvey::_switchToGenerator( const CircularSurvey::PtrGenerator &newG) { if (this->_pGenerator != newG) { @@ -270,8 +202,6 @@ void CircularSurvey::_changeVariant() { } void CircularSurvey::_updateWorker() { - // Mark transects as dirty. - this->_transectsDirty = true; // Reset data. this->_transects.clear(); this->_variantVector.clear(); @@ -385,12 +315,6 @@ void CircularSurvey::_reverseWorker() { } } -void CircularSurvey::applyNewAltitude(double newAltitude) { - _cameraCalc.valueSetIsDistance()->setRawValue(true); - _cameraCalc.distanceToSurface()->setRawValue(newAltitude); - _cameraCalc.setDistanceToSurfaceRelative(true); -} - double CircularSurvey::timeBetweenShots() { return 1; } QString CircularSurvey::commandDescription() const { @@ -401,9 +325,18 @@ QString CircularSurvey::commandName() const { return tr("Circular Survey"); } QString CircularSurvey::abbreviation() const { return tr("C.S."); } -bool CircularSurvey::readyForSave() const { - return TransectStyleComplexItem::readyForSave() && !_transectsDirty && - this->_state == STATE::IDLE; +TransectStyleComplexItem::ReadyForSaveState +CircularSurvey::readyForSaveState() const { + if (TransectStyleComplexItem::readyForSaveState() == + TransectStyleComplexItem::ReadyForSaveState::ReadyForSave) { + if (this->_state == STATE::IDLE) { + return ReadyForSaveState::ReadyForSave; + } else { + return ReadyForSaveState::NotReadyForSaveData; + } + } else { + return TransectStyleComplexItem::readyForSaveState(); + } } double CircularSurvey::additionalTimeDelay() const { return 0; } @@ -546,18 +479,6 @@ void CircularSurvey::_rebuildTransectsPhase1(void) { << " ms"; } -void CircularSurvey::_recalcComplexDistance() { - _complexDistance = 0; - if (_transectsDirty) - return; - for (int i = 0; i < _visualTransectPoints.count() - 1; i++) { - _complexDistance += - _visualTransectPoints[i].value().distanceTo( - _visualTransectPoints[i + 1].value()); - } - emit complexDistanceChanged(); -} - // no cameraShots in Circular Survey, add if desired void CircularSurvey::_recalcCameraShots() { _cameraShots = 0; } @@ -666,9 +587,6 @@ void CircularSurvey::_setTransects(CircularSurvey::PtrRoutingData pRoute) { &CircularSurvey::_changeVariant); this->_changeVariantWorker(); - // Mark transect ready. - this->_transectsDirty = false; - this->_state = STATE::SKIPP; this->_rebuildTransects(); } else { diff --git a/src/Wima/CircularSurvey.h b/src/Wima/CircularSurvey.h index de7253a2b29e1982b281c8d545ae5d751c951ab2..fb2cb0872f1301f8b7e931fb0a7e65b18d0f4a76 100644 --- a/src/Wima/CircularSurvey.h +++ b/src/Wima/CircularSurvey.h @@ -23,15 +23,12 @@ class CircularSurvey : public TransectStyleComplexItem { using PtrGenerator = std::shared_ptr; using PtrRoutingData = std::shared_ptr; - using PtrWorker = std::shared_ptr; + using PtrWorker = std::unique_ptr; + using Transects = QList>; + using Variant = Transects; public: - /// @param vehicle Vehicle which this is being contructed for - /// @param flyView true: Created for use in the Fly View, false: Created for - /// use in the Plan View - /// @param kmlOrShpFile Polygon comes from this file, empty for default - /// polygon - CircularSurvey(Vehicle *vehicle, bool flyView, const QString &kmlOrShpFile, + CircularSurvey(PlanMasterController *masterController, bool flyView, const QString &kmlOrShpFile, QObject *parent); ~CircularSurvey(); @@ -63,14 +60,11 @@ public: QString mapVisualQML(void) const override final; void save(QJsonArray &planItems) override final; bool specifiesCoordinate(void) const override final; - void appendMissionItems(QList &items, - QObject *missionItemParent) override final; - void applyNewAltitude(double newAltitude) override final; double timeBetweenShots(void) override final; QString commandDescription(void) const override final; QString commandName(void) const override final; QString abbreviation(void) const override final; - bool readyForSave(void) const override final; + ReadyForSaveState readyForSaveState(void) const override final; double additionalTimeDelay(void) const override final; // Generator @@ -97,7 +91,6 @@ signals: private slots: // Overrides from TransectStyleComplexItem void _rebuildTransectsPhase1(void) final; - void _recalcComplexDistance(void) final; void _recalcCameraShots(void) final; // Worker functions. @@ -108,11 +101,6 @@ private slots: void _reverseWorker(); private: - void _appendLoadedMissionItems(QList &items, - QObject *missionItemParent); - void _buildAndAppendMissionItems(QList &items, - QObject *missionItemParent); - bool _switchToGenerator(const PtrGenerator &newG); // State. @@ -139,7 +127,6 @@ private: PtrGenerator _pGenerator; // Routing. - using Variant = Transects; QVector _variantVector; PtrWorker _pWorker; }; diff --git a/src/Wima/Geometry/WimaArea.cc b/src/Wima/Geometry/WimaArea.cc index fcca7d2e43977d148824f912b89866ffa46bbda5..d4681a10500fd455c2afbd609fc1bd0dda855bc7 100644 --- a/src/Wima/Geometry/WimaArea.cc +++ b/src/Wima/Geometry/WimaArea.cc @@ -269,7 +269,7 @@ bool WimaArea::join(const WimaArea &area1, const WimaArea &area2, } else if (retValue == JoinPolygonError::PathSizeLow) { qWarning("Polygon vertex count is low."); } else { - QVector path; + QList path; toGeoList(joinedPolygon, origin, path); // qWarning("after transform"); // qWarning() << path; @@ -491,38 +491,6 @@ void WimaArea::init() { &WimaArea::recalcInteractivity); } -/*! - * \fn void print(const WimaArea &area) - * Prints the data contained in \a area to the console. - */ -void print(const WimaArea &area) { - QString message; - print(area, message); - qWarning() << message; -} - -/*! - * \fn void print(const WimaArea &area) - * Prints the data contained in \a area to the \a outputString. - */ -void print(const WimaArea &area, QString &outputString) { - outputString.append(QString("Type: %1\n").arg(area.objectName())); - print(static_cast(area), outputString); - outputString.append(QString("Maximum Altitude: %1\n").arg(area._maxAltitude)); - outputString.append( - QString("Border Polygon Offset: %1\n") - .arg(area._borderPolygonOffset.rawValue().toDouble())); - - outputString.append( - QString("Border Polygon Coordinates\n") - .arg(area._borderPolygonOffset.rawValue().toDouble())); - for (int i = 0; i < area._borderPolygon.count(); i++) { - QGeoCoordinate coordinate = area._borderPolygon.vertexCoordinate(i); - outputString.append( - QString("%1\n").arg(coordinate.toString(QGeoCoordinate::Degrees))); - } -} - // QDoc Documentation /*! diff --git a/src/Wima/Geometry/WimaMeasurementArea.cc b/src/Wima/Geometry/WimaMeasurementArea.cc index e02bef9ff177b74144be8b56eb9e5d07d94aff7f..eee40a95dd1984f2e32816f095cc1acf13e2ef7f 100644 --- a/src/Wima/Geometry/WimaMeasurementArea.cc +++ b/src/Wima/Geometry/WimaMeasurementArea.cc @@ -20,7 +20,7 @@ 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 *obj = other.tiles[i]; const auto *tile = qobject_cast(obj); if (tile != nullptr) { this->tiles.append(new SnakeTile(*tile, this)); @@ -33,8 +33,17 @@ TileData &TileData::operator=(const TileData &other) { } bool TileData::operator==(const TileData &other) const { - return this->tiles == other.tiles && - this->tileCenterPoints == other.tileCenterPoints; + if (this->tileCenterPoints == other.tileCenterPoints && + this->tiles.count() == other.tiles.count()) { + for (int i = 0; i < other.tiles.count(); ++i) { + if (this->tiles[i] != other.tiles[i]) { + return false; + } + } + return true; + } else { + return false; + } } bool TileData::operator!=(const TileData &other) const { @@ -72,7 +81,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 +100,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 +166,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."; } @@ -233,68 +242,68 @@ void WimaMeasurementArea::doUpdate() { auto start = std::chrono::high_resolution_clock::now(); 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); + 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->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/WimaServiceArea.cc b/src/Wima/Geometry/WimaServiceArea.cc index a1b58314d3059bf4f7026f664fe8200be94ad917..d73163e12ac9a9edb68ebd0490edbc9e1a0e3b33 100644 --- a/src/Wima/Geometry/WimaServiceArea.cc +++ b/src/Wima/Geometry/WimaServiceArea.cc @@ -1,5 +1,9 @@ #include "WimaServiceArea.h" +#include "QGCLoggingCategory.h" + +QGC_LOGGING_CATEGORY(WimaServiceAreaLog, "WimaServiceAreaLog") + const char *WimaServiceArea::wimaServiceAreaName = "Service Area"; const char *WimaServiceArea::depotLatitudeName = "DepotLatitude"; const char *WimaServiceArea::depotLongitudeName = "DepotLongitude"; @@ -102,20 +106,24 @@ void WimaServiceArea::init() { } else if (this->_depot.isValid()) { // Use nearest coordinate. auto minDist = std::numeric_limits::infinity(); - auto minIt = this->pathReference().begin(); - for (auto it = this->pathReference().begin(); - it < this->pathReference().end(); ++it) { - const auto vertex = it->value(); - auto d = vertex.distanceTo(this->_depot); - if (d < minDist) { - minDist = d; - minIt = it; + int minIndex = 0; + for (int idx = 0; idx < this->pathModel().count(); ++idx) { + const QObject *obj = this->pathModel()[idx]; + const auto *vertex = qobject_cast(obj); + if (vertex != nullptr) { + auto d = vertex->distanceTo(this->_depot); + if (d < minDist) { + minDist = d; + minIndex = idx; + } + } else { + qCCritical(WimaServiceAreaLog) << "init(): nullptr catched!"; } } - this->setDepot(minIt->value()); - } else if (this->pathReference().size() > 0) { + this->setDepot(*this->pathModel().value(minIndex)); + } else if (this->pathModel().count() > 0) { // Use first coordinate. - this->setDepot(this->pathReference().value(0).value()); + this->setDepot(*this->pathModel().value(0)); } } }); diff --git a/src/Wima/Snake/CircularGenerator.cpp b/src/Wima/Snake/CircularGenerator.cpp index 129e0e8e681858f1030517dd85cc8008a107f48e..a4e39375830f82cce5196b5d2437ebde3eb70a5e 100644 --- a/src/Wima/Snake/CircularGenerator.cpp +++ b/src/Wima/Snake/CircularGenerator.cpp @@ -99,7 +99,9 @@ bool CircularGenerator::get(Generator &generator) { if (progress.size() == tiles->count()) { for (int i = 0; i < tiles->count(); ++i) { if (progress[i] == 100) { - const auto *tile = tiles->value(i); + const auto *obj = (*tiles)[int(i)]; + const auto *tile = qobject_cast(obj); + if (tile != nullptr) { snake::FPolygon tileENU; snake::areaToEnu(origin, tile->coordinateList(), tileENU); diff --git a/src/Wima/Snake/LinearGenerator.cpp b/src/Wima/Snake/LinearGenerator.cpp index 4a81934750c4eb232f4a05fad2f7cadc164fba2b..91bfe9f0f2c7bc3d662bcb0a3d205ffad84a745c 100644 --- a/src/Wima/Snake/LinearGenerator.cpp +++ b/src/Wima/Snake/LinearGenerator.cpp @@ -74,7 +74,9 @@ bool LinearGenerator::get(Generator &generator) { if (progress.size() == tiles->count()) { for (int i = 0; i < tiles->count(); ++i) { if (progress[i] == 100) { - const auto *tile = tiles->value(i); + const QObject *obj = (*tiles)[int(i)]; + const auto *tile = qobject_cast(obj); + if (tile != nullptr) { snake::FPolygon tileENU; snake::areaToEnu(origin, tile->coordinateList(), tileENU); diff --git a/src/Wima/Snake/NemoInterface.cpp b/src/Wima/Snake/NemoInterface.cpp index 95b5d753e59e4d0c008cf86bcfbf75809361fe86..dd07f8d3999ed8127796f5562e89f869698951d3 100644 --- a/src/Wima/Snake/NemoInterface.cpp +++ b/src/Wima/Snake/NemoInterface.cpp @@ -97,7 +97,7 @@ private: }; using StatusMap = std::map; -StatusMap statusMap{ +static StatusMap statusMap{ std::make_pair( NemoInterface::STATUS::NOT_CONNECTED, "Not Connected"), std::make_pair( @@ -121,16 +121,14 @@ NemoInterface::Impl::Impl(NemoInterface *p) 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 + qgcApp()->warningMessageBoxOnMainThread( + "Nemo Interface", + "Websocket connection string possibly invalid: " + connectionString + + ". Trying to connect anyways."); } + this->pRosBridge.reset( + new ros_bridge::ROSBridge(connectionString.toLocal8Bit().data())); }; connect(connectionStringFact, &SettingsFact::rawValueChanged, setConnectionString); @@ -158,33 +156,33 @@ void NemoInterface::Impl::setTileData(const TileData &tileData) { UniqueLock lk1(this->ENUOriginMutex, std::adopt_lock); UniqueLock lk2(this->tilesENUMutex, std::adopt_lock); - const auto *obj = tileData.tiles.get(0); + const auto *obj = tileData.tiles[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 { + 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[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 { + } else { qCDebug(NemoInterfaceLog) << "Impl::setTileData(): Origin invalid."; - } + } } else { qCDebug(NemoInterfaceLog) << "Impl::setTileData(): tile empty."; + } } - } } else { this->tileData.clear(); @@ -255,7 +253,8 @@ void NemoInterface::Impl::doTopicServiceSetup() { progressMsg.progress().size() != requiredSize) { // Some error occured. progressMsg.progress().clear(); - qgcApp()->showMessage("Invalid progress message received."); + qgcApp()->informationMessageBoxOnMainThread( + "Nemo Interface", "Invalid progress message received."); } emit this->parent->progressChanged(); @@ -307,7 +306,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 +329,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 +349,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 +394,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 +407,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/WaypointManager/DefaultManager.cpp b/src/Wima/WaypointManager/DefaultManager.cpp index 97fd1e7c93e8398acb00d39ac94977dde30339f8..eca4e4f1e6e8a0833c37c96759f17eadfdfc77a0 100644 --- a/src/Wima/WaypointManager/DefaultManager.cpp +++ b/src/Wima/WaypointManager/DefaultManager.cpp @@ -53,7 +53,7 @@ bool WaypointManager::DefaultManager::_insertMissionItem( using namespace WaypointManager::Utils; if (!insertMissionItem(c, index /*insertion index*/, list, - _settings->vehicle(), _settings->isFlyView(), + _settings->masterController(), _settings->isFlyView(), &list /*parent*/, doUpdate /*do update*/)) { qWarning( "WaypointManager::DefaultManager::next(): insertMissionItem failed."); @@ -117,7 +117,7 @@ bool WaypointManager::DefaultManager::_worker() { } _currentMissionItems.clearAndDeleteContents(); - initialize(_currentMissionItems, _settings->vehicle(), + initialize(_currentMissionItems, _settings->masterController(), _settings->isFlyView()); // Calculate path from home to first waypoint. @@ -171,7 +171,7 @@ bool WaypointManager::DefaultManager::_worker() { Q_ASSERT(takeoffItem != nullptr); return false; } - makeTakeOffCmd(takeoffItem, _settings->masterController()->managerVehicle()); + makeTakeOffCmd(takeoffItem, _settings->masterController()); // Create change speed item. _insertMissionItem(_settings->homePosition(), 2 /*insertion index*/, @@ -241,7 +241,7 @@ bool WaypointManager::DefaultManager::_worker() { qWarning("WaypointManager::DefaultManager::next(): nullptr."); return false; } - makeLandCmd(landItem, _settings->masterController()->managerVehicle()); + makeLandCmd(landItem, _settings->masterController()); // Set altitude. for (int i = 1; i < _currentMissionItems.count(); ++i) { diff --git a/src/Wima/WaypointManager/RTLManager.cpp b/src/Wima/WaypointManager/RTLManager.cpp index 3218e74f5d40dcee2593e6fd111b6d4cb5d473ba..88b6fc253ba81493bae9fa4bf6ab4dffa1c43106 100644 --- a/src/Wima/WaypointManager/RTLManager.cpp +++ b/src/Wima/WaypointManager/RTLManager.cpp @@ -88,7 +88,7 @@ bool WaypointManager::RTLManager::_insertMissionItem(const QGeoCoordinate &c, using namespace WaypointManager::Utils; if (!insertMissionItem(c, index /*insertion index*/, list, - _settings->vehicle(), _settings->isFlyView(), + _settings->masterController(), _settings->isFlyView(), &list /*parent*/, doUpdate /*do update*/)) { qWarning("WaypointManager::RTLManager::next(): insertMissionItem failed."); Q_ASSERT(false); @@ -133,7 +133,7 @@ bool WaypointManager::RTLManager::_worker() { } _currentMissionItems.clearAndDeleteContents(); - initialize(_currentMissionItems, _settings->vehicle(), + initialize(_currentMissionItems, _settings->masterController(), _settings->isFlyView()); // Calculate path from vehicle to home position. @@ -188,7 +188,7 @@ bool WaypointManager::RTLManager::_worker() { qWarning("WaypointManager::RTLManager::next(): nullptr."); return false; } - makeLandCmd(landItem, _settings->masterController()->managerVehicle()); + makeLandCmd(landItem, _settings->masterController()); // Set altitude. for (int i = 1; i < _currentMissionItems.count(); ++i) { diff --git a/src/Wima/WaypointManager/Utils.cpp b/src/Wima/WaypointManager/Utils.cpp index ce4c576054e9aa4f012183af156958f67ec46b74..87d4b19463c799e57225b3c66740829c700870a7 100644 --- a/src/Wima/WaypointManager/Utils.cpp +++ b/src/Wima/WaypointManager/Utils.cpp @@ -1,20 +1,16 @@ #include "Utils.h" #include "MissionSettingsItem.h" +#include "PlanMasterController.h" + #include #include -template<> -QVariant WaypointManager::Utils::getCoordinate(const SimpleMissionItem* item) -{ - return QVariant::fromValue(item->coordinate()); -} - bool WaypointManager::Utils::insertMissionItem(const QGeoCoordinate &coordinate, long index, QmlObjectListModel &list, - Vehicle* vehicle, + PlanMasterController* masterController, bool flyView, QObject *parent, bool doUpdates) @@ -26,7 +22,7 @@ bool WaypointManager::Utils::insertMissionItem(const QGeoCoordinate &coordinate, } else { if (parent == nullptr) parent = &list; - SimpleMissionItem * newItem = new SimpleMissionItem(vehicle, flyView, parent); + auto *newItem = new SimpleMissionItem(masterController, flyView, MissionItem(), parent); int sequenceNumber = detail::nextSequenceNumber(list); newItem->setSequenceNumber(sequenceNumber); @@ -159,10 +155,10 @@ bool WaypointManager::Utils::updateHomePosition(QmlObjectListModel &list) } void WaypointManager::Utils::initialize(QmlObjectListModel &list, - Vehicle *vehicle, + PlanMasterController *masterController, bool flyView) { - MissionSettingsItem* settingsItem = new MissionSettingsItem(vehicle, flyView, &list); + MissionSettingsItem* settingsItem = new MissionSettingsItem(masterController, flyView, &list); list.insert(0, settingsItem); } @@ -189,26 +185,6 @@ void WaypointManager::Utils::printList(QmlObjectListModel &list) } } -bool WaypointManager::Utils::makeTakeOffCmd(SimpleMissionItem *item, Vehicle *vehicle) -{ - if ( vehicle->fixedWing() - || vehicle->vtol() - || vehicle->multiRotor() - ) - { - MAV_CMD takeoffCmd = vehicle->vtol() - ? MAV_CMD_NAV_VTOL_TAKEOFF - : MAV_CMD_NAV_TAKEOFF; - if (vehicle->firmwarePlugin()->supportedMissionCommands().contains(takeoffCmd)) { - auto c = item->coordinate(); - item->setCommand(takeoffCmd); - item->setCoordinate(c); - return true; - } - } - return false; -} - void WaypointManager::Utils::makeSpeedCmd(SimpleMissionItem *item, double speed) { assert(speed > 0); @@ -218,16 +194,3 @@ void WaypointManager::Utils::makeSpeedCmd(SimpleMissionItem *item, double speed) item->setCoordinate(c); item->missionItem().setParam2(speed); } - -bool WaypointManager::Utils::makeLandCmd(SimpleMissionItem *item, Vehicle *vehicle) -{ - MAV_CMD landCmd = vehicle->vtol() ? MAV_CMD_NAV_VTOL_LAND : MAV_CMD_NAV_LAND; - if (vehicle->firmwarePlugin()->supportedMissionCommands().contains(landCmd)) { - item->setCommand(landCmd); - } else { - Q_ASSERT(false); - qWarning("WaypointManager::Utils::makeLandCmd(): Land command not supported!"); - return false; - } - return true; -} diff --git a/src/Wima/WaypointManager/Utils.h b/src/Wima/WaypointManager/Utils.h index 2351f98740634d45897ae42439a0fb59dadb4d11..cf9ab61ed8daf5cdccd23ea4f1d3d19a3bff9d1b 100644 --- a/src/Wima/WaypointManager/Utils.h +++ b/src/Wima/WaypointManager/Utils.h @@ -5,126 +5,106 @@ #include - -class Vehicle; +class PlanMasterController; class QGeoCoordinate; - - namespace WaypointManager { namespace Utils { - -template -CoordinateType getCoordinate(const SimpleMissionItem* item){ - return item->coordinate(); +template +CoordinateType getCoordinate(const SimpleMissionItem *item) { + return item->coordinate(); } -template<> -QVariant getCoordinate(const SimpleMissionItem* item); - +template <> QVariant getCoordinate(const SimpleMissionItem *item) { + return QVariant::fromValue(item->coordinate()); +} -/// extracts the coordinates stored in missionItems (list of MissionItems) and stores them in coordinateList. +/// extracts the coordinates stored in missionItems (list of MissionItems) and +/// stores them in coordinateList. template class ContainerType> bool extract(const QmlObjectListModel &missionItems, ContainerType &coordinateList, - std::size_t startIndex, - std::size_t endIndex) -{ - - if ( startIndex < std::size_t(missionItems.count()) - && endIndex < std::size_t(missionItems.count()) - && missionItems.count() > 0) { - if (startIndex > endIndex) { - if ( !extract(missionItems, - coordinateList, - startIndex, - std::size_t(missionItems.count()-1)) /*recursion*/) - return false; - if ( !extract(missionItems, - coordinateList, - 0, - endIndex) /*recursion*/) - return false; - } else { - for (std::size_t i = startIndex; i <= endIndex; ++i) { - SimpleMissionItem *mItem = missionItems.value(int(i)); - - if (mItem == nullptr) { - coordinateList.clear(); - return false; - } - coordinateList.append(getCoordinate(mItem)); - } - } - } else + std::size_t startIndex, std::size_t endIndex) { + + if (startIndex < std::size_t(missionItems.count()) && + endIndex < std::size_t(missionItems.count()) && + missionItems.count() > 0) { + if (startIndex > endIndex) { + if (!extract(missionItems, coordinateList, startIndex, + std::size_t(missionItems.count() - 1)) /*recursion*/) return false; + if (!extract(missionItems, coordinateList, 0, endIndex) /*recursion*/) + return false; + } else { + for (std::size_t i = startIndex; i <= endIndex; ++i) { + const QObject *obj = missionItems[int(i)]; + auto *mItem = qobject_cast(obj); + + if (mItem == nullptr) { + coordinateList.clear(); + return false; + } + coordinateList.append(getCoordinate(mItem)); + } + } + } else + return false; - return true; + return true; } -/// extracts the coordinates stored in missionItems (list of MissionItems) and stores them in coordinateList. +/// extracts the coordinates stored in missionItems (list of MissionItems) and +/// stores them in coordinateList. template class ContainerType> bool extract(const QmlObjectListModel &missionItems, - ContainerType &coordinateList) -{ - - return extract(missionItems, - coordinateList, - std::size_t(0), - std::size_t(missionItems.count()) - ); -} + ContainerType &coordinateList) { + return extract(missionItems, coordinateList, std::size_t(0), + std::size_t(missionItems.count())); +} -/// extracts the coordinates stored in missionItems (list of MissionItems) and stores them in coordinateList. +/// extracts the coordinates stored in missionItems (list of MissionItems) and +/// stores them in coordinateList. template class ContainerType> -bool extract(const ContainerType &source, - ContainerType &destination, - std::size_t startIndex, - std::size_t endIndex) -{ - - if ( startIndex < std::size_t(source.size()) - && endIndex < std::size_t(source.size()) - && source.size() > 0) { - if (startIndex > endIndex) { - if ( !extract(source, - destination, - startIndex, - std::size_t(long(source.size())-1)) /*recursion*/) - return false; - if ( !extract(source, - destination, - 0, - endIndex) /*recursion*/) - return false; - } else { - for (std::size_t i = startIndex; i <= endIndex; ++i) { - destination.append(source[i]); - } - } - } else +bool extract(const ContainerType &source, + ContainerType &destination, std::size_t startIndex, + std::size_t endIndex) { + + if (startIndex < std::size_t(source.size()) && + endIndex < std::size_t(source.size()) && source.size() > 0) { + if (startIndex > endIndex) { + if (!extract(source, destination, startIndex, + std::size_t(long(source.size()) - 1)) /*recursion*/) return false; - - return true; + if (!extract(source, destination, 0, endIndex) /*recursion*/) + return false; + } else { + for (std::size_t i = startIndex; i <= endIndex; ++i) { + destination.append(source[i]); + } + } + } else + return false; + + return true; } //! //! \brief Makes the SimpleMissionItem \p item a takeoff command. //! \param item SimpleMissionItem -//! \param vehilce Vehicle. //! \return Returns true if successfull, false either. //! -bool makeTakeOffCmd(SimpleMissionItem *item, Vehicle *vehicle); +bool makeTakeOffCmd(SimpleMissionItem *item, + PlanMasterController *masterController); //! //! \brief Makes the SimpleMissionItem \p item a land command. //! \param item SimpleMissionItem -//! \param vehilce Vehicle. //! \return Returns true if successfull, false either. //! -bool makeLandCmd(SimpleMissionItem *item, Vehicle *vehicle); +bool makeLandCmd(SimpleMissionItem *item, + PlanMasterController *masterController); //! //! \brief Makes the SimpleMissionItem \p item a MAV_CMD_DO_CHANGE_SPEED item. @@ -133,58 +113,56 @@ bool makeLandCmd(SimpleMissionItem *item, Vehicle *vehicle); //! void makeSpeedCmd(SimpleMissionItem *item, double speed); - //! //! \brief Initializes the list \p list. //! //! Adds a MissionSettingsItem to \p list. //! \param list List of VisualMissionItems. -//! \param vehicle Vehicle. //! \param flyView Fly- or planview. void initialize(QmlObjectListModel &list, - Vehicle *vehicle, - bool flyView=true); + PlanMasterController *masterController, bool flyView = true); -//! @brief Creates (from QGeoCoordinate) and inserts a SimpleMissionItem at the given index to list. -//! \param coordinate Coordinate of the SimpleMissionItem. +//! @brief Creates (from QGeoCoordinate) and inserts a SimpleMissionItem at the +//! given index to list. \param coordinate Coordinate of the SimpleMissionItem. //! \param index Index to insert SimpleMissionItem. //! \param list List of SimpleMissionItems. -//! \param vehicle Vehicle. //! \param flyView Fly- or planview. //! \param parent Parent of the SimpleMissionItem (Set to &list if nullptr). -//! \param doUpdates Neccessary update are performed to the list \p list if set to true. Set this to true for the last item only, -//! if inserting multiple items. -//! \return Returns true on success, false either. +//! \param doUpdates Neccessary update are performed to the list \p list if set +//! to true. Set this to true for the last item only, if inserting multiple +//! items. \return Returns true on success, false either. //! //! @note The list must be initialized. (\see \fn initialize) -//! @note This function doesn't establish any signal-slot connections (\see \fn MissionController::_initVisualItem). -//! @note This function doesn't set the MissionFlightStatus_t (\see \fn VisualMissionItem::setMissionFlightStatus). -bool insertMissionItem(const QGeoCoordinate &coordinate, - long index, +//! @note This function doesn't establish any signal-slot connections (\see \fn +//! MissionController::_initVisualItem). +//! @note This function doesn't set the MissionFlightStatus_t (\see \fn +//! VisualMissionItem::setMissionFlightStatus). +bool insertMissionItem(const QGeoCoordinate &coordinate, long index, QmlObjectListModel &list, - Vehicle *vehicle, - bool flyView=true, - QObject *parent=nullptr, - bool doUpdates=true); + PlanMasterController *masterController, + bool flyView = true, QObject *parent = nullptr, + bool doUpdates = true); //! //! \brief Performs a list update. //! -//! Updates the home position of the MissionSettingsItem, the parent child hirarchy and the sequence numbers. -//! \param list List containing VisualMissionItems. -//! \return Returns true if no errors occured, false either. +//! Updates the home position of the MissionSettingsItem, the parent child +//! hirarchy and the sequence numbers. \param list List containing +//! VisualMissionItems. \return Returns true if no errors occured, false either. bool updateList(QmlObjectListModel &list); -//! @brief Updates the sequence numbers of the VisualMissionItems inside \p list. +//! @brief Updates the sequence numbers of the VisualMissionItems inside \p +//! list. //! //! \param list List containing VisualMissionItems. -//! \param firstSeqNumber The sequence number of the fisrt list entry (defaul = 0). -//! \return Returns true if successfull, false either. +//! \param firstSeqNumber The sequence number of the fisrt list entry (defaul = +//! 0). \return Returns true if successfull, false either. //! -//! \note If the list \p list contains only one VisualMissionItem it's sequence number is -//! set to 0. +//! \note If the list \p list contains only one VisualMissionItem it's sequence +//! number is set to 0. bool updateSequenceNumbers(QmlObjectListModel &list, size_t firstSeqNumber = 0); -//! @brief Update the parent child item hierarchy of the VisualMissionItems inside \p list. +//! @brief Update the parent child item hierarchy of the VisualMissionItems +//! inside \p list. //! //! \param list List containing VisualMissionItems. //! \return Returns true if successfull, false either. @@ -192,7 +170,8 @@ bool updateSequenceNumbers(QmlObjectListModel &list, size_t firstSeqNumber = 0); //! \note Returns true on success, false either. bool updateHirarchy(QmlObjectListModel &list); -//! @brief Updates the home position to the first valid coordinate of the VisualMissionItems inside \p list. +//! @brief Updates the home position to the first valid coordinate of the +//! VisualMissionItems inside \p list. //! //! \param list List containing VisualMissionItems. //! \return Returns true if the home position was updated, false either. @@ -204,11 +183,9 @@ bool updateHomePosition(QmlObjectListModel &list); void printList(QmlObjectListModel &list); namespace detail { - size_t nextSequenceNumber(QmlObjectListModel &list); - bool previousAltitude(QmlObjectListModel &list, - size_t index, - double &altitude, - int &altitudeMode); +size_t nextSequenceNumber(QmlObjectListModel &list); +bool previousAltitude(QmlObjectListModel &list, size_t index, double &altitude, + int &altitudeMode); } // namespace detail } // namespace Utils diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index 7c9a050b06138c90293b8c9827802221699fb58b..422e786b4f9e53acb544ef6753cf4672a6899980 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -33,8 +33,6 @@ constexpr typename std::underlying_type::type integral(T value) { return static_cast::type>(value); } -#define SMART_RTL_MAX_ATTEMPTS 3 // times -#define SMART_RTL_ATTEMPT_INTERVAL 200 // ms #define EVENT_TIMER_INTERVAL 50 // ms const char *WimaController::areaItemsName = "AreaItems"; @@ -55,23 +53,7 @@ WimaController::WimaController(QObject *parent) _metaDataMap(FactMetaData::createMapFromJsonFile( QStringLiteral(":/json/WimaController.SettingsGroup.json"), this)), _enableWimaController(settingsGroup, - _metaDataMap[enableWimaControllerName]), - _flightSpeed(settingsGroup, _metaDataMap[flightSpeedName]), - _altitude(settingsGroup, _metaDataMap[altitudeName]), - _lowBatteryHandlingTriggered(false), - _batteryLevelTicker(EVENT_TIMER_INTERVAL, 1000 /*ms*/) { - - // Set up facts for waypoint manager. - connect(&_flightSpeed, &Fact::rawValueChanged, this, - &WimaController::_updateflightSpeed); - connect(&_altitude, &Fact::rawValueChanged, this, - &WimaController::_updateAltitude); - - // Periodic. - connect(&_eventTimer, &QTimer::timeout, this, - &WimaController::_eventTimerHandler); - _eventTimer.start(EVENT_TIMER_INTERVAL); - + _metaDataMap[enableWimaControllerName]){ // PlanData and Progress. connect(WimaBridge::instance(), &WimaBridge::planDataChanged, this, &WimaController::planDataChangedHandler); @@ -115,16 +97,6 @@ void WimaController::setMissionController(MissionController *missionC) { emit missionControllerChanged(); } -void WimaController::requestSmartRTL() { - qCWarning(WimaControllerLog) << "requestSmartRTL() called"; - QString errorString("Smart RTL requested."); - if (!_SRTLPrecondition(errorString)) { - qgcApp()->showMessage(errorString); - return; - } - emit smartRTLRequestConfirm(); -} - bool WimaController::upload() { auto &items = _currentWM->currentMissionItems(); if (_masterController && _masterController->managerVehicle() && @@ -141,68 +113,6 @@ bool WimaController::upload() { } } -bool WimaController::forceUpload() { - auto ¤tMissionItems = _currentWM->currentMissionItems(); - if (currentMissionItems.count() < 1 || !_missionController || - !_masterController) { - qCWarning(WimaControllerLog) - << "forceUpload(): error:" - << "currentMissionItems.count(): " << currentMissionItems.count() - << "_missionController: " << _missionController - << "_masterController: " << _masterController; - return false; - } else { - _missionController->removeAll(); - // Set homeposition of settingsItem. - QmlObjectListModel *visuals = _missionController->visualItems(); - MissionSettingsItem *settingsItem = - visuals->value(0); - if (settingsItem == nullptr) { - Q_ASSERT(false); - qCWarning(WimaControllerLog) << "updateCurrentMissionItems(): nullptr"; - return false; - } else { - settingsItem->setCoordinate(_WMSettings.homePosition()); - - // Copy mission items to _missionController and send them. - for (int i = 1; i < currentMissionItems.count(); i++) { - auto *item = currentMissionItems.value(i); - _missionController->insertSimpleMissionItem(*item, visuals->count()); - } - _masterController->sendToVehicle(); - return true; - } - } -} - -void WimaController::removeFromVehicle() { - if (_masterController && _missionController) { - _masterController->removeAllFromVehicle(); - _missionController->removeAll(); - } -} - -void WimaController::executeSmartRTL() { - qCWarning(WimaControllerLog) << "executeSmartRTL() called"; - if (_masterController && _masterController->managerVehicle()) { - forceUpload(); - _masterController->managerVehicle()->startMission(); - } -} - -void WimaController::initSmartRTL() { - qCWarning(WimaControllerLog) << "initSmartRTL() called"; - _initSmartRTL(); -} - -void WimaController::removeVehicleTrajectoryHistory() { - if (_masterController && _masterController->managerVehicle()) { - _masterController->managerVehicle() - ->trajectoryPoints() - ->clearAndDeleteContents(); - } -} - bool WimaController::_calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QVector &path) { @@ -312,109 +222,3 @@ void WimaController::_updateAltitude() { emit missionItemsChanged(); emit waypointPathChanged(); } - -void WimaController::_checkBatteryLevel() { - if (_missionController && _masterController && - _masterController->managerVehicle()) { - Vehicle *managerVehicle = masterController()->managerVehicle(); - WimaSettings *wimaSettings = - qgcApp()->toolbox()->settingsManager()->wimaSettings(); - int threshold = wimaSettings->lowBatteryThreshold()->rawValue().toInt(); - bool enabled = _enableWimaController.rawValue().toBool(); - unsigned int minTime = - wimaSettings->minimalRemainingMissionTime()->rawValue().toUInt(); - - if (enabled) { - Fact *battery1percentRemaining = - managerVehicle->battery1FactGroup()->getFact( - VehicleBatteryFactGroup::_percentRemainingFactName); - Fact *battery2percentRemaining = - managerVehicle->battery2FactGroup()->getFact( - VehicleBatteryFactGroup::_percentRemainingFactName); - - if (battery1percentRemaining->rawValue().toDouble() < threshold && - battery2percentRemaining->rawValue().toDouble() < threshold) { - if (!_lowBatteryHandlingTriggered) { - _lowBatteryHandlingTriggered = true; - if (!(_missionController->remainingTime() <= minTime)) { - requestSmartRTL(); - } - } - } else { - _lowBatteryHandlingTriggered = false; - } - } - } -} - -void WimaController::_eventTimerHandler() { - // Battery level check necessary? - Fact *enableLowBatteryHandling = qgcApp() - ->toolbox() - ->settingsManager() - ->wimaSettings() - ->enableLowBatteryHandling(); - if (enableLowBatteryHandling->rawValue().toBool() && - _batteryLevelTicker.ready()) - _checkBatteryLevel(); -} - -void WimaController::_smartRTLCleanUp(bool flying) { - if (!flying && _missionController) { // vehicle has landed - _switchWaypointManager(_emptyWM); - _missionController->removeAll(); - disconnect(masterController()->managerVehicle(), &Vehicle::flyingChanged, - this, &WimaController::_smartRTLCleanUp); - } -} - -bool WimaController::_SRTLPrecondition(QString &errorString) { - if (!_planDataValid) { - errorString.append(tr("No WiMA data available. Please define at least a " - "measurement and a service area.")); - return false; - } - - return _rtlWM.checkPrecondition(errorString); -} - -void WimaController::_switchWaypointManager( - WaypointManager::ManagerBase &manager) { - if (_currentWM != &manager) { - _currentWM = &manager; - emit missionItemsChanged(); - emit waypointPathChanged(); - - qCWarning(WimaControllerLog) << "_switchWaypointManager: statistics update " - "missing."; - } -} - -void WimaController::_initSmartRTL() { - static int attemptCounter = 0; - attemptCounter++; - QString errorString; - - if (_SRTLPrecondition(errorString)) { - if (_missionController && _masterController && - _masterController->managerVehicle()) { - _masterController->managerVehicle()->pauseVehicle(); - connect(_masterController->managerVehicle(), &Vehicle::flyingChanged, - this, &WimaController::_smartRTLCleanUp); - if (_rtlWM.update()) { // Calculate return path. - _switchWaypointManager(_rtlWM); - removeFromVehicle(); - attemptCounter = 0; - emit smartRTLPathConfirm(); - } - } - } else if (attemptCounter > SMART_RTL_MAX_ATTEMPTS) { - errorString.append( - tr("Smart RTL: No success after maximum number of attempts.")); - qgcApp()->showMessage(errorString); - attemptCounter = 0; - } else { - _smartRTLTimer.singleShot(SMART_RTL_ATTEMPT_INTERVAL, this, - &WimaController::_initSmartRTL); - } -} diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index 09f6b4d9228270b6f37e266e4454ea293a261661..2b359e75b3dd4bca27d922a01f972a357851bd03 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -70,10 +70,6 @@ public: // Member Methodes Q_INVOKABLE WimaController *thisPointer(); - // Smart RTL. - Q_INVOKABLE void requestSmartRTL(); - Q_INVOKABLE void initSmartRTL(); - Q_INVOKABLE void executeSmartRTL(); // Other. Q_INVOKABLE void removeVehicleTrajectoryHistory(); Q_INVOKABLE bool upload(); @@ -97,9 +93,6 @@ signals: // Waypoints. void missionItemsChanged(void); void waypointPathChanged(void); - // Smart RTL. - void smartRTLRequestConfirm(void); - void smartRTLPathConfirm(void); // Upload. void forceUploadConfirm(void); @@ -111,11 +104,6 @@ private slots: QVector &path); void _updateflightSpeed(void); void _updateAltitude(void); - // SMART RTL - void _checkBatteryLevel(void); - bool _SRTLPrecondition(QString &errorString); - void _initSmartRTL(); - void _smartRTLCleanUp(bool flying); // Snake. void _switchWaypointManager(WaypointManager::ManagerBase &manager); // Periodic tasks. @@ -152,10 +140,6 @@ private: SettingsFact _flightSpeed; // mission flight speed SettingsFact _altitude; // mission altitude - // Smart RTL. - QTimer _smartRTLTimer; - bool _lowBatteryHandlingTriggered; - // Periodic tasks. QTimer _eventTimer; EventTicker _batteryLevelTicker; diff --git a/src/Wima/WimaPlaner.cc b/src/Wima/WimaPlaner.cc index 3d9dd4c753998d7b83561ff8f4e7ec9798ef0168..7e276747d6bf2437f7a9047866a7f73c009bc2cd 100644 --- a/src/Wima/WimaPlaner.cc +++ b/src/Wima/WimaPlaner.cc @@ -39,9 +39,9 @@ const char *WimaPlaner::missionItemsName = "MissionItems"; WimaPlaner::WimaPlaner(QObject *parent) : QObject(parent), _masterController(nullptr), _missionController(nullptr), _currentAreaIndex(-1), _joinedArea(this), _survey(nullptr), - _synchronized(false), _nemoInterface(this), - _stateMachine(new StateMachine), _areasMonitored(false), - _missionControllerMonitored(false), _progressLocked(false) { + _nemoInterface(this), _stateMachine(new StateMachine), + _areasMonitored(false), _missionControllerMonitored(false), + _progressLocked(false), _synchronized(false) { connect(this, &WimaPlaner::currentPolygonIndexChanged, this, &WimaPlaner::updatePolygonInteractivity); @@ -286,37 +286,43 @@ void WimaPlaner::_update() { // Check if polygons have at least three vertices if (_serviceArea.count() < 3) { - qgcApp()->showMessage(tr("Service area has less than three vertices.")); + qgcApp()->warningMessageBoxOnMainThread( + tr("Area Error"), tr("Service area has less than three vertices.")); return; } if (_measurementArea.count() < 3) { - qgcApp()->showMessage( + qgcApp()->warningMessageBoxOnMainThread( + tr("Area Error"), 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")); + qgcApp()->warningMessageBoxOnMainThread( + tr("Area Error"), tr("Service area is not a simple polygon. Only " + "simple polygons allowed.")); return; } if (!_corridor.isSimplePolygon() && _corridor.count() > 0) { - qgcApp()->showMessage(tr("Corridor is not a simple polygon. Only " - "simple polygons allowed.\n")); + qgcApp()->warningMessageBoxOnMainThread( + tr("Area Error"), tr("Corridor is not a simple polygon. Only simple " + "polygons allowed.")); return; } if (!_measurementArea.isSimplePolygon()) { - qgcApp()->showMessage(tr("Measurement area is not a simple " - "polygon. Only simple polygons allowed.\n")); + qgcApp()->warningMessageBoxOnMainThread( + tr("Area Error"), tr("Measurement area is not a simple polygon. Only " + "simple polygons allowed.")); return; } if (!_serviceArea.containsCoordinate(_serviceArea.depot())) { - qgcApp()->showMessage(tr("Depot not inside service area.")); + qgcApp()->warningMessageBoxOnMainThread( + tr("Area Error"), tr("Depot not inside service area.")); return; } @@ -326,7 +332,8 @@ void WimaPlaner::_update() { _joinedArea.join(_corridor); } if (!_joinedArea.join(_measurementArea)) { - qgcApp()->showMessage( + qgcApp()->warningMessageBoxOnMainThread( + tr("Area Error"), tr("Not able to join areas. Service and measurement area" " must be overlapping, or connected through a " "corridor.")); @@ -380,9 +387,9 @@ void WimaPlaner::_update() { // Remove old arrival and return path. int size = missionItems->count(); for (int i = surveyIndex + 1; i < size; i++) - _missionController->removeMissionItem(surveyIndex + 1); + _missionController->removeVisualItem(surveyIndex + 1); for (int i = surveyIndex - 1; i > 1; i--) - _missionController->removeMissionItem(i); + _missionController->removeVisualItem(i); // set home position to serArea center MissionSettingsItem *settingsItem = @@ -398,20 +405,7 @@ void WimaPlaner::_update() { 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); + _missionController->insertTakeoffItem(depot, 1, false); if (_survey->visualTransectPoints().size() == 0) { qCWarning(WimaPlanerLog) << "update(): survey no points"; @@ -423,11 +417,10 @@ void WimaPlaner::_update() { 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()); + qgcApp()->warningMessageBoxOnMainThread( + tr("Path Error"), tr("Not able to calculate path from " + "depot position to measurement area. Please " + "double check area and route parameters.")); return; } @@ -441,11 +434,10 @@ void WimaPlaner::_update() { 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()); + qgcApp()->warningMessageBoxOnMainThread( + tr("Path Error"), tr("Not able to calculate path from " + "measurement area to depot position. Please " + "double check area and route parameters.")); return; } @@ -458,16 +450,11 @@ void WimaPlaner::_update() { (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; + (void)_missionController->insertLandItem(depot, missionItems->count(), + true); + + auto surveyIndex = _missionController->visualItems()->indexOf(_survey); + _missionController->setCurrentPlanViewSeqNum(surveyIndex, true); this->_stateMachine->updateState(EVENT::PATH_UPDATED); } @@ -576,7 +563,8 @@ void WimaPlaner::saveToFile(const QString &filename) { QFile file(planFilename); if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) { - qgcApp()->showMessage( + qgcApp()->warningMessageBoxOnMainThread( + tr("Save Error"), tr("Plan save error %1 : %2").arg(filename).arg(file.errorString())); _currentFile.clear(); emit currentFileChanged(); @@ -589,9 +577,11 @@ void WimaPlaner::saveToFile(const QString &filename) { fileType = FileType::PlanFile; } else { if (planFilename.contains(".")) { - qgcApp()->showMessage(tr("File format not supported")); + qgcApp()->warningMessageBoxOnMainThread( + tr("Save Error"), tr("File format not supported")); } else { - qgcApp()->showMessage(tr("File without file extension not accepted.")); + qgcApp()->warningMessageBoxOnMainThread( + tr("Save Error"), tr("File without file extension not accepted.")); return; } } @@ -642,7 +632,8 @@ bool WimaPlaner::loadFromFile(const QString &filename) { if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) { errorString = file.errorString() + QStringLiteral(" ") + filename; - qgcApp()->showMessage(errorMessage.arg(errorString)); + qgcApp()->warningMessageBoxOnMainThread(tr("Load Error"), + errorMessage.arg(errorString)); return false; } @@ -651,7 +642,8 @@ bool WimaPlaner::loadFromFile(const QString &filename) { QByteArray bytes = file.readAll(); if (!JsonHelper::isJsonFile(bytes, jsonDoc, errorString)) { - qgcApp()->showMessage(errorMessage.arg(errorString)); + qgcApp()->warningMessageBoxOnMainThread(tr("Load Error"), + errorMessage.arg(errorString)); return false; } @@ -671,7 +663,8 @@ bool WimaPlaner::loadFromFile(const QString &filename) { bool success = _measurementArea.loadFromJson(jsonArea, errorString); if (!success) { - qgcApp()->showMessage(errorMessage.arg(errorString)); + qgcApp()->warningMessageBoxOnMainThread( + tr("Load Error"), errorMessage.arg(errorString)); return false; } @@ -683,7 +676,8 @@ bool WimaPlaner::loadFromFile(const QString &filename) { bool success = _serviceArea.loadFromJson(jsonArea, errorString); if (!success) { - qgcApp()->showMessage(errorMessage.arg(errorString)); + qgcApp()->warningMessageBoxOnMainThread( + tr("Load Error"), errorMessage.arg(errorString)); return false; } @@ -695,7 +689,8 @@ bool WimaPlaner::loadFromFile(const QString &filename) { bool success = _corridor.loadFromJson(jsonArea, errorString); if (!success) { - qgcApp()->showMessage(errorMessage.arg(errorString)); + qgcApp()->warningMessageBoxOnMainThread( + tr("Load Error"), errorMessage.arg(errorString)); return false; } @@ -705,7 +700,8 @@ bool WimaPlaner::loadFromFile(const QString &filename) { } else { errorString += QString(tr("%s not supported.\n").arg(WimaArea::areaTypeName)); - qgcApp()->showMessage(errorMessage.arg(errorString)); + qgcApp()->warningMessageBoxOnMainThread( + tr("Load Error"), errorMessage.arg(errorString)); return false; } } else { @@ -776,7 +772,8 @@ bool WimaPlaner::loadFromFile(const QString &filename) { return true; } else { errorString += QString(tr("File extension not supported.\n")); - qgcApp()->showMessage(errorMessage.arg(errorString)); + qgcApp()->warningMessageBoxOnMainThread(tr("Load Error"), + errorMessage.arg(errorString)); return false; } }