diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 40feba8b9c0f250edace97a1a71b761240df344d..edb83f2a540c90d22ecc57dfbeb4ea2468dba29b 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -431,23 +431,20 @@ HEADERS += \ src/Snake/mapbox/variant_io.hpp \ src/Snake/snake.h \ src/Snake/snake_typedefs.h \ + src/Wima/Geometry/GenericPolygon.h \ + src/Wima/Geometry/GenericPolygonArray.h \ src/Wima/Geometry/GeoPoint3D.h \ - src/Wima/Geometry/Polygon2D.h \ - src/Wima/Geometry/PolygonArray.h \ - src/Wima/Snake/GeoPolygonArray.h \ - src/Wima/Snake/GeoTile.h \ - src/Wima/Snake/PolygonArray.h \ src/Wima/Snake/QNemoHeartbeat.h \ src/Wima/Snake/QNemoProgress.h \ src/Wima/Snake/QNemoProgress.h \ + src/Wima/Snake/SnakeTile.h \ + src/Wima/Snake/SnakeTileLocal.h \ src/Wima/Snake/SnakeTiles.h \ src/Wima/Snake/SnakeTilesLocal.h \ - src/Wima/Snake/SnakeWorker.h \ + src/Wima/Snake/SnakeW.h \ src/Wima/WaypointManager/AreaInterface.h \ src/Wima/WaypointManager/DefaultManager.h \ src/Wima/WaypointManager/GenericWaypointManager.h \ - src/Wima/Geometry/WimaPolygonArray.h \ - src/Wima/Snake/snaketile.h \ src/Wima/WaypointManager/RTLManager.h \ src/Wima/WaypointManager/Settings.h \ src/Wima/WaypointManager/Slicer.h \ @@ -473,7 +470,6 @@ HEADERS += \ src/Wima/WimaPlanData.h \ src/Wima/Geometry/WimaJoinedArea.h \ src/Wima/Geometry/WimaJoinedAreaData.h \ - src/Wima/Geometry/SphereCalculus.h \ src/Wima/CircularSurveyComplexItem.h \ src/Wima/Geometry/PlanimetryCalculus.h \ src/Wima/Geometry/Circle.h \ @@ -504,10 +500,9 @@ SOURCES += \ src/Snake/clipper/clipper.cpp \ src/Snake/snake.cpp \ src/Wima/Geometry/GeoPoint3D.cpp \ - src/Wima/Geometry/PolygonArray.cc \ - src/Wima/Snake/GeoTile.cpp \ src/Wima/Snake/QNemoProgress.cc \ - src/Wima/Snake/SnakeWorker.cc \ + src/Wima/Snake/SnakeDataManager.cc \ + src/Wima/Snake/SnakeTile.cpp \ src/Wima/WaypointManager/AreaInterface.cpp \ src/Wima/WaypointManager/DefaultManager.cpp \ src/Wima/WaypointManager/GenericWaypointManager.cpp \ @@ -530,7 +525,6 @@ SOURCES += \ src/comm/ros_bridge/include/server.cpp \ src/comm/ros_bridge/include/topic_publisher.cpp \ src/comm/ros_bridge/include/topic_subscriber.cpp \ - src/Wima/Snake/snaketile.cpp \ src/api/QGCCorePlugin.cc \ src/api/QGCOptions.cc \ src/api/QGCSettings.cc \ @@ -550,7 +544,6 @@ SOURCES += \ src/Wima/Geometry/WimaMeasurementAreaData.cc \ src/Wima/Geometry/WimaJoinedArea.cc \ src/Wima/Geometry/WimaJoinedAreaData.cc \ - src/Wima/Geometry/SphereCalculus.cc \ src/Wima/CircularSurveyComplexItem.cc \ src/Wima/Geometry/PlanimetryCalculus.cc \ src/Wima/Geometry/Circle.cc \ diff --git a/src/Snake/snake.cpp b/src/Snake/snake.cpp index b7ae331b9c7be65e50ab26c510b51c71fc83c07e..f02dac519f3c6175d3fe05df3e3820ac4804a7bb 100644 --- a/src/Snake/snake.cpp +++ b/src/Snake/snake.cpp @@ -446,6 +446,21 @@ const BoostPolygon &Scenario::corridor() const return _corridor; } +BoostPolygon &Scenario::measurementArea() +{ + return _mArea; +} + +BoostPolygon &Scenario::serviceArea() +{ + return _sArea; +} + +BoostPolygon &Scenario::corridor() +{ + return _corridor; +} + const BoostPolygon &Scenario::joinedArea() const { return _jArea; } diff --git a/src/Snake/snake.h b/src/Snake/snake.h index 95fbb557e5e90ce9f9af35a8e19a6cfeeb5ff056..eec0dce1262e89e0483c9fab7022119957d0e64f 100644 --- a/src/Snake/snake.h +++ b/src/Snake/snake.h @@ -168,14 +168,18 @@ class Scenario{ const BoostPolygon &serviceArea()const; const BoostPolygon &corridor()const; - double tileWidth() const; - void setTileWidth(double tileWidth); + BoostPolygon &measurementArea(); + BoostPolygon &serviceArea(); + BoostPolygon &corridor(); + + Length tileWidth() const; + void setTileWidth(Length tileWidth); - double tileHeight() const; - void setTileHeight(double tileHeight); + Length tileHeight() const; + void setTileHeight(Length tileHeight); - double minTileArea() const; - void setMinTileArea(double minTileArea); + Area minTileArea() const; + void setMinTileArea(Area minTileArea); const BoostPolygon &joinedArea() const; const vector &tiles() const; @@ -192,9 +196,9 @@ private: bool _calculateTiles(); bool _calculateJoinedArea(); - double _tileWidth; - double _tileHeight; - double _minTileArea; + Length _tileWidth; + Length _tileHeight; + Area _minTileArea; mutable bool _needsUpdate; diff --git a/src/Snake/snake_typedefs.h b/src/Snake/snake_typedefs.h index 7788928384fbbc19c7e829f507f24099d6fa11a7..db5b2a7504c0214cb3cdca47dcb0fe2826503147 100644 --- a/src/Snake/snake_typedefs.h +++ b/src/Snake/snake_typedefs.h @@ -3,8 +3,10 @@ #include #include #include +#include namespace bg = boost::geometry; +namespace bu = boost::units; namespace snake { typedef bg::model::point BoostPoint; @@ -13,4 +15,7 @@ typedef bg::model::polygon BoostPolygon; typedef bg::model::linestring BoostLineString; typedef std::vector> Int64Matrix; +typedef bu::quantity Length; +typedef bu::quantity Area; + } diff --git a/src/Wima/Geometry/Polygon2D.h b/src/Wima/Geometry/GenericPolygon.h similarity index 68% rename from src/Wima/Geometry/Polygon2D.h rename to src/Wima/Geometry/GenericPolygon.h index d9390a5ad4cc795572dfb558eeb2f025e22ceddd..974f49da6d504627f3f60b04bfe7f057643b6cee 100644 --- a/src/Wima/Geometry/Polygon2D.h +++ b/src/Wima/Geometry/GenericPolygon.h @@ -5,16 +5,14 @@ #include "ros_bridge/include/messages/geometry_msgs/polygon_stamped.h" -namespace polygon_stamped = ros_bridge::messages::geometry_msgs::polygon_stamped; - template class ContainerType = QVector> -class Polygon2DTemplate{ // +class GenericPolygon{ // typedef ros_bridge::messages::geometry_msgs::polygon::GenericPolygon Polygon; public: - Polygon2DTemplate(){} - Polygon2DTemplate(const Polygon2DTemplate &other) : _polygon(other._polygon){} + GenericPolygon(){} + GenericPolygon(const GenericPolygon &other) : _polygon(other._polygon){} - Polygon2DTemplate& operator=(const Polygon2DTemplate& other) { + GenericPolygon& operator=(const GenericPolygon& other) { this->_polygon = other._polygon; return *this; } @@ -29,5 +27,3 @@ private: Polygon _polygon; }; - -typedef Polygon2DTemplate<> Polygon2D; diff --git a/src/Wima/Geometry/WimaPolygonArray.h b/src/Wima/Geometry/GenericPolygonArray.h similarity index 83% rename from src/Wima/Geometry/WimaPolygonArray.h rename to src/Wima/Geometry/GenericPolygonArray.h index 359bf8c4667f0ab386040686a542c6fd0b40441f..68dfa14ae0afa5490df03e7fb57d348919347a12 100644 --- a/src/Wima/Geometry/WimaPolygonArray.h +++ b/src/Wima/Geometry/GenericPolygonArray.h @@ -6,14 +6,14 @@ #include template class ContainerType = QVector> -class WimaPolygonArray +class GenericPolygonArray { public: - WimaPolygonArray() {} - WimaPolygonArray(const WimaPolygonArray &other) : + GenericPolygonArray() {} + GenericPolygonArray(const GenericPolygonArray &other) : _polygons(other._polygons), _dirty(true) {} - ~WimaPolygonArray(){ + ~GenericPolygonArray(){ _objs.clearAndDeleteContents(); } @@ -33,7 +33,7 @@ public: return _polygons; } - WimaPolygonArray &operator =(const WimaPolygonArray &other){ + GenericPolygonArray &operator =(const GenericPolygonArray &other){ this->_polygons = other._polygons; this->_dirty = true; return *this; diff --git a/src/Wima/Geometry/PolygonArray.cc b/src/Wima/Geometry/PolygonArray.cc deleted file mode 100644 index 4dac28602f0b3424bf4bdfbbc3d64ae293284cf6..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/PolygonArray.cc +++ /dev/null @@ -1 +0,0 @@ -#include "PolygonArray.h" diff --git a/src/Wima/Geometry/PolygonArray.h b/src/Wima/Geometry/PolygonArray.h deleted file mode 100644 index 3d92547e2a4541f1db50bcaa65cbd6fce7348830..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/PolygonArray.h +++ /dev/null @@ -1,12 +0,0 @@ -#pragma once - -#include - -template class ContainerType > -class PolygonArray : public ContainerType { -public: - explicit PolygonArray() : ContainerType() {} - PolygonArray(const PolygonArray &other) : ContainerType(other) {} - - QString type() const override {return "PolygonArray";} -}; diff --git a/src/Wima/Geometry/SphereCalculus.cc b/src/Wima/Geometry/SphereCalculus.cc deleted file mode 100644 index 504d101eb10f8b0dbff91b0f97d09c11d721512c..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/SphereCalculus.cc +++ /dev/null @@ -1,36 +0,0 @@ -#include "SphereCalculus.h" - - - - - -/*! - \class SphereCalculus - \inmodule Wima - - \brief The \c WimaArea class provides the a base class for - all areas used within the Wima extension. - - \c WimaArea uses a \l {Simple Polygon} derived from \c {QGCMapPolygon} - to define areas inside which certain taskts are performed. The polygon (often refered to as the path) can - be displayed visually on a map. -*/ - - - -/*! - \externalpage https://en.wikipedia.org/wiki/Simple_polygon - \title Simple Polygon -*/ - -/*! - \externalpage https://en.wikipedia.org/wiki/Dijkstra%27s_algorithm - \title Dijkstra Algorithm -*/ - - - - - - - diff --git a/src/Wima/Geometry/SphereCalculus.h b/src/Wima/Geometry/SphereCalculus.h deleted file mode 100644 index c595514cdcb02fc4aca1e9b493a2fc1ba3112a69..0000000000000000000000000000000000000000 --- a/src/Wima/Geometry/SphereCalculus.h +++ /dev/null @@ -1,19 +0,0 @@ -#ifndef SphereCalculus_H -#define SphereCalculus_H -#endif // SphereCalculus_H - -#include -#include -#include -#include - -#include "QGCGeo.h" - -#include "PlanimetryCalculus.h" - -namespace SphereCalculus { - - -} - - diff --git a/src/Wima/Geometry/WimaAreaData.cc b/src/Wima/Geometry/WimaAreaData.cc index 099ef51cb230469eb8a6d490497dac324ab21dfe..a1fafe82e17e380a14468b1e64260766a568cc0c 100644 --- a/src/Wima/Geometry/WimaAreaData.cc +++ b/src/Wima/Geometry/WimaAreaData.cc @@ -55,6 +55,11 @@ void WimaAreaData::append(const QGeoCoordinate &c) { _path.push_back(QVariant::fromValue(c)); } +void WimaAreaData::push_back(const QGeoCoordinate &c) +{ + append(c); +} + void WimaAreaData::clear() { _list.clear(); _path.clear(); diff --git a/src/Wima/Geometry/WimaAreaData.h b/src/Wima/Geometry/WimaAreaData.h index afadd6f2e302ce2bead961e70aee670300fd6c67..da3ba9365f4013b2773a998fdd74c5c0606aaa42 100644 --- a/src/Wima/Geometry/WimaAreaData.h +++ b/src/Wima/Geometry/WimaAreaData.h @@ -26,6 +26,7 @@ public: virtual QString type() const = 0; void append(const QGeoCoordinate &c); + void push_back(const QGeoCoordinate &c); void clear(); signals: diff --git a/src/Wima/Snake/GeoPolygonArray.h b/src/Wima/Snake/GeoPolygonArray.h deleted file mode 100644 index d7a969c3c747c9d4099967037218f81326d07b1a..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/GeoPolygonArray.h +++ /dev/null @@ -1,5 +0,0 @@ -#pragma once -#include "GeoTile.h" -#include "Wima/Geometry/WimaPolygonArray.h" - -using GeoPolygonArray = WimaPolygonArray; diff --git a/src/Wima/Snake/GeoTile.cpp b/src/Wima/Snake/GeoTile.cpp deleted file mode 100644 index e383644dcb1527edf2bd032b57e46c6170a71d2f..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/GeoTile.cpp +++ /dev/null @@ -1,23 +0,0 @@ -#include "GeoTile.h" - -GeoTile::GeoTile() : WimaAreaData() -{ - -} - -GeoTile::GeoTile(const GeoTile &other) : WimaAreaData() -{ - *this = other; -} - -GeoTile &GeoTile::operator=(const GeoTile &other) -{ - this->assign(other); - return *this; - -} - -void GeoTile::assign(const GeoTile &other) -{ - WimaAreaData::assign(other); -} diff --git a/src/Wima/Snake/GeoTile.h b/src/Wima/Snake/GeoTile.h deleted file mode 100644 index 1471934ca04cd48b7e0aeb9cc54274f5d9071efd..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/GeoTile.h +++ /dev/null @@ -1,19 +0,0 @@ -#pragma once - -#include "Wima/Geometry/WimaAreaData.h" - -class GeoTile : public WimaAreaData -{ -public: - GeoTile(); - GeoTile(const GeoTile&other); - - QString type() const {return "Tile";} - GeoTile* Clone() const {return new GeoTile(*this);} - - GeoTile& operator=(const GeoTile &other); - -protected: - void assign(const GeoTile &other); -}; - diff --git a/src/Wima/Snake/PolygonArray.h b/src/Wima/Snake/PolygonArray.h deleted file mode 100644 index ceebbacd9f0ef7fd003ee3831ba13cdd9f2d165c..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/PolygonArray.h +++ /dev/null @@ -1,6 +0,0 @@ -#pragma once - -#include "Wima/Geometry/Polygon2D.h" -#include "Wima/Geometry/WimaPolygonArray.h" - -typedef WimaPolygonArray SnakeTilesLocal; diff --git a/src/Wima/Snake/SnakeDataManager.cc b/src/Wima/Snake/SnakeDataManager.cc new file mode 100644 index 0000000000000000000000000000000000000000..fb92c3286f99851d524e89137fcfdd095785c0e5 --- /dev/null +++ b/src/Wima/Snake/SnakeDataManager.cc @@ -0,0 +1,344 @@ +#include "SnakeDataManager.h" + +#include +#include + +#include "QGCApplication.h" +#include "SettingsManager.h" +#include "QGCToolbox.h" +#include "WimaSettings.h" +#include "SettingsFact.h" + +#include +#include +#include + +#include "snake.h" +#include "Wima/Snake/SnakeTiles.h" +#include "Wima/Snake/SnakeTilesLocal.h" + +#include "comm/ros_bridge/include/ros_bridge.h" + +using QVariantList = QList; +using ROSBridgePtr = std::unique_ptr; + +using UniqueLock = std::unique_lock; +using SharedLock = std::shared_lock; + +class SnakeImpl : public QObject{ + Q_OBJECT +public: + SnakeImpl() : + lineDistance(1*si::meter) + , minTransectLength(1*si::meter) + , calcInProgress(false) + , topicServiceSetupDone(false) + { + // 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 { + qgcApp()->showMessage("ROS Bridge connection string invalid: " + connectionString); + this->pRosBridge.reset(new ros_bridge::ROSBridge("localhost:9090")); + } + }; + setConnectionString(); + connect(connectionStringFact, &SettingsFact::rawValueChanged, setConnectionString); + } + + // Private data. + ROSBridgePtr pRosBridge; + bool topicServiceSetupDone; + mutable std::shared_timed_mutex mutex; + + // Input + snake::Scenario scenario; + Length lineDistance; + Length minTransectLength; + QList mArea; + QList sArea; + QList corridor; + + + // Output + std::atomic_bool calcInProgress; + QNemoProgress progress; + QNemoHeartbeat heartbeat; + + QVector waypoints; + QVector arrivalPath; + QVector returnPath; + QGeoCoordinate ENUorigin; + QVector waypointsENU; + QVector arrivalPathENU; + QVector returnPathENU; + SnakeTiles tiles; + QVariantList tileCenterPoints; + SnakeTilesLocal tilesENU; + QVector tileCenterPointsENU; + QString errorMessage; +}; + +template +class ToggleRAII{ +public: + ToggleRAII(AtomicType &t) + : _t(t) + {} + + ~ToggleRAII() + { + if ( _t.load() ){ + _t.store(false); + } else { + _t.store(true); + } + } +private: + AtomicType &_t; +}; + + + +SnakeDataManager::SnakeDataManager(QObject *parent) + : QThread(parent) + , _pImpl(std::make_unique()) + +{ +} + +SnakeDataManager::~SnakeDataManager() +{ + +} + +void SnakeDataManager::setMeasurementArea(const QList &measurementArea) +{ + UniqueLock lk(this->_pImpl->m); + this->_pImpl->mArea = measurementArea; +} + +void SnakeDataManager::setServiceArea(const QList &serviceArea) +{ + UniqueLock lk(this->_pImpl->m); + this->_pImpl->sArea = serviceArea; +} + +void SnakeDataManager::setCorridor(const QList &corridor) +{ + UniqueLock lk(this->_pImpl->m); + this->_pImpl->corridor = corridor; +} + +QNemoProgress SnakeDataManager::progress() +{ + SharedLock lk(this->_pImpl->m); + return this->_pImpl->progress; +} + +QNemoHeartbeat SnakeDataManager::heartbeat() +{ + SharedLock lk(this->_pImpl->m); + return this->_pImpl->heartbeat; +} + +bool SnakeDataManager::calcInProgress() +{ + return this->_pImpl->calcInProgress.load(); +} + +Length SnakeDataManager::lineDistance() const +{ + SharedLock lk(this->_pImpl->m); + return this->_pImpl->lineDistance; +} + +void SnakeDataManager::setLineDistance(Length lineDistance) +{ + UniqueLock lk(this->_pImpl->m); + this->_pImpl->lineDistance = lineDistance; +} + +Area SnakeDataManager::minTileArea() const +{ + SharedLock lk(this->_pImpl->m); + return this->_pImpl->scenario.minTileArea(); +} + +void SnakeDataManager::setMinTileArea(Area minTileArea) +{ + UniqueLock lk(this->_pImpl->m); + this->_pImpl->scenario.setMinTileArea(minTileArea); +} + +Length SnakeDataManager::tileHeight() const +{ + SharedLock lk(this->_pImpl->m); + return this->_pImpl->scenario.tileHeight(); +} + +void SnakeDataManager::setTileHeight(Length tileHeight) +{ + UniqueLock lk(this->_pImpl->m); + this->_pImpl->scenario.setTileHeight(tileHeight); +} + +Length SnakeDataManager::tileWidth() const +{ + SharedLock lk(this->_pImpl->m); + return this->_pImpl->scenario.tileWidth(); +} + +void SnakeDataManager::setTileWidth(Length tileWidth) +{ + UniqueLock lk(this->_pImpl->m); + this->_pImpl->scenario.setTileWidth(tileWidth); +} + +bool SnakeDataManager::precondition() const +{ + return true; +} + +void SnakeDataManager::run() +{ + this->_pImpl->calcInProgress.store(true); + ToggleRAII tr(this->_pImpl->calcInProgress); + + UniqueLock lk(&_pImpl->m); + _pImpl->clear(); + + if ( !precondition() ) + return; + + if ( this->_pImpl->mArea.size() < 3) { + _pImpl->errorMessage = "Measurement area invalid: size < 3."; + return; + } + if ( this->_pImpl->sArea.size() < 3) { + _pImpl->errorMessage = "Service area invalid: size < 3."; + return; + } + + + || _sArea.size() <= 0 + || _corridor.size() <= 0 ){ + + auto origin = _mArea.front(); + // Measurement area update necessary. + if ( _mArea.size() != _pScenario->measurementArea().outer().size() ){ + { + QMutexLocker lk(&_pImpl->m); + _pImpl->ENUorigin = origin; + } + for (auto geoVertex : _mArea){ + snake::BoostPoint p; + snake::toENU(origin, geoVertex, p); + _pScenario->measurementArea().outer().push_back(p); + } + } + + // Service area update necessary. + if ( _sArea.size() != _pScenario->serviceArea().outer().size() ){ + for (auto geoVertex : _sArea){ + snake::BoostPoint p; + snake::toENU(origin, geoVertex, p); + _pScenario->serviceArea().outer().push_back(p); + } + } + + // Service area update necessary. + if ( _corridor.size() != _pScenario->corridor().outer().size() ){ + for (auto geoVertex : _corridor){ + snake::BoostPoint p; + snake::toENU(origin, geoVertex, p); + _pScenario->corridor().outer().push_back(p); + } + } + + if ( !_pScenario->update() ){ + { + QMutexLocker lk(&_pImpl->m); + for (auto c : _pScenario->errorString){ + _pImpl->errorMessage.push_back(QChar(c)); + } + } + return; + } + + // Asynchronously update flightplan. + auto future = std::async(std::bind(&snake::Flightplan::update, _pFlightplan)); + + // Continue with storing scenario data in the mean time. + { + QMutexLocker lk(&_pImpl->m); + // Get tiles. + const auto &tiles = _pScenario->tiles(); + const auto ¢erPoints = _pScenario->tileCenterPoints(); + for ( unsigned int i=0; i < tiles.size(); ++i ) { + const auto &tile = tiles[i]; + SnakeTile geoTile; + SnakeTileLocal enuTile; + for ( size_t i = tile.outer().size(); i < tile.outer().size()-1; ++i) { + auto &p = tile.outer()[i]; + QPointF enuVertex(p.get<0>(), p.get<1>()); + QGeoCoordinate geoVertex; + snake::fromENU(origin, p, geoVertex); + enuTile.polygon().points().push_back(enuVertex); + geoTile.push_back(geoVertex); + } + const auto &boostPoint = centerPoints[i]; + QPointF enuVertex(boostPoint.get<0>(), boostPoint.get<1>()); + QGeoCoordinate geoVertex; + snake::fromENU(origin, boostPoint, geoVertex); + geoTile.setCenter(geoVertex); + _pImpl->tiles.polygons().push_back(geoTile); + _pImpl->tileCenterPoints.push_back(QVariant::fromValue(geoVertex)); + _pImpl->tilesENU.polygons().push_back(enuTile); + _pImpl->tileCenterPointsENU.push_back(enuVertex); + } + } + + future.wait(); + // Trying to generate flight plan. + if ( !future.get() ){ + // error + QMutexLocker lk(&_pImpl->m); + for (auto c : _pFlightplan->errorString){ + _pImpl->errorMessage.push_back(QChar(c)); + } + } else { + //success!!! + QMutexLocker lk(&_pImpl->m); + // Store waypoints. + for (auto boostVertex : _pFlightplan->waypoints()){ + QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()}; + QGeoCoordinate geoVertex; + snake::fromENU(origin, boostVertex, geoVertex); + _pImpl->waypointsENU.push_back(enuVertex); + _pImpl->waypoints.push_back(geoVertex); + } + // Store arrival path. + for (auto boostVertex : _pFlightplan->arrivalPath()){ + QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()}; + QGeoCoordinate geoVertex; + snake::fromENU(origin, boostVertex, geoVertex); + _pImpl->arrivalPathENU.push_back(enuVertex); + _pImpl->arrivalPath.push_back(geoVertex); + } + // Store return path. + for (auto boostVertex : _pFlightplan->returnPath()){ + QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()}; + QGeoCoordinate geoVertex; + snake::fromENU(origin, boostVertex, geoVertex); + _pImpl->returnPathENU.push_back(enuVertex); + _pImpl->returnPath.push_back(geoVertex); + } + } +} + + diff --git a/src/Wima/Snake/SnakeDataManager.h b/src/Wima/Snake/SnakeDataManager.h new file mode 100644 index 0000000000000000000000000000000000000000..69ec9aa000d297b32e0d9d75beaf5d2b2c9127a7 --- /dev/null +++ b/src/Wima/Snake/SnakeDataManager.h @@ -0,0 +1,70 @@ +#pragma once + +#include +#include +#include +#include + +#include "QNemoProgress.h" +#include "QNemoHeartbeat.h" + +#include > + + +using namespace boost::units; +using Length = quantity; +using Area = quantity; + +class SnakeImpl; + +class SnakeDataManager : public QThread{ + Q_OBJECT + +public: + using SnakeImplPtr = std::unique_ptr; + + SnakeDataManager(QObject *parent = nullptr); + ~SnakeDataManager() override; + + + void setMeasurementArea (const QList &measurementArea); + void setServiceArea (const QList &serviceArea); + void setCorridor (const QList &corridor); + + QNemoProgress progress(); + QNemoHeartbeat heartbeat(); + bool calcInProgress(); + + Length lineDistance() const; + void setLineDistance(Length lineDistance); + + Length minTransectLength() const; + void setMinTransectLength(Length minTransectLength); + + Area minTileArea() const; + void setMinTileArea(Area minTileArea); + + Length tileHeight() const; + void setTileHeight(Length tileHeight); + + Length tileWidth() const; + void setTileWidth(Length tileWidth); + + + +protected: + void run() override; + +private: + bool precondition() const; + SnakeImplPtr _pImpl; +}; + + + + + + + + + diff --git a/src/Wima/Snake/SnakeTile.cpp b/src/Wima/Snake/SnakeTile.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a1c73a2fea14f2a6dce06896d8802ca7d6bfb1bc --- /dev/null +++ b/src/Wima/Snake/SnakeTile.cpp @@ -0,0 +1,23 @@ +#include "SnakeTile.h" + +SnakeTile::SnakeTile() : WimaAreaData() +{ + +} + +SnakeTile::SnakeTile(const SnakeTile &other) : WimaAreaData() +{ + *this = other; +} + +SnakeTile &SnakeTile::operator=(const SnakeTile &other) +{ + this->assign(other); + return *this; + +} + +void SnakeTile::assign(const SnakeTile &other) +{ + WimaAreaData::assign(other); +} diff --git a/src/Wima/Snake/SnakeTile.h b/src/Wima/Snake/SnakeTile.h new file mode 100644 index 0000000000000000000000000000000000000000..aeb6a7e7c6fd23a62b9469d09f62e40081e5936e --- /dev/null +++ b/src/Wima/Snake/SnakeTile.h @@ -0,0 +1,19 @@ +#pragma once + +#include "Wima/Geometry/WimaAreaData.h" + +class SnakeTile : public WimaAreaData +{ +public: + SnakeTile(); + SnakeTile(const SnakeTile&other); + + QString type() const {return "Tile";} + SnakeTile* Clone() const {return new SnakeTile(*this);} + + SnakeTile& operator=(const SnakeTile &other); + +protected: + void assign(const SnakeTile &other); +}; + diff --git a/src/Wima/Snake/SnakeTileLocal.h b/src/Wima/Snake/SnakeTileLocal.h new file mode 100644 index 0000000000000000000000000000000000000000..c1bcd2bb64f5d4482cd46a469636e2b2a32f37b3 --- /dev/null +++ b/src/Wima/Snake/SnakeTileLocal.h @@ -0,0 +1,4 @@ +#pragma once +#include "Wima/Geometry/GenericPolygon.h" + +using SnakeTileLocal = GenericPolygon<>; diff --git a/src/Wima/Snake/SnakeTiles.h b/src/Wima/Snake/SnakeTiles.h new file mode 100644 index 0000000000000000000000000000000000000000..e73c46022233d25bd98909c9a85a264c774c5d34 --- /dev/null +++ b/src/Wima/Snake/SnakeTiles.h @@ -0,0 +1,5 @@ +#pragma once +#include "SnakeTile.h" +#include "Wima/Geometry/GenericPolygonArray.h" + +using SnakeTiles = GenericPolygonArray; diff --git a/src/Wima/Snake/SnakeTilesLocal.h b/src/Wima/Snake/SnakeTilesLocal.h new file mode 100644 index 0000000000000000000000000000000000000000..54c8225d6b5a8be20b970fabc0e94ae453b5083b --- /dev/null +++ b/src/Wima/Snake/SnakeTilesLocal.h @@ -0,0 +1,6 @@ +#pragma once + +#include "Wima/Snake/SnakeTileLocal.h" +#include "Wima/Geometry/GenericPolygonArray.h" + +typedef GenericPolygonArray SnakeTilesLocal; diff --git a/src/Wima/Snake/SnakeWorker.cc b/src/Wima/Snake/SnakeWorker.cc deleted file mode 100644 index 767942a3987a9e238bafd7aa8d7cd2b3c91c233a..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/SnakeWorker.cc +++ /dev/null @@ -1,273 +0,0 @@ -#include "SnakeWorker.h" -#include -#include - - -#include "snake.h" - - -SnakeDataManager::SnakeDataManager(QObject *parent) - : QThread(parent) - , _pScenario(std::make_shared()) - , _pProgress(std::make_shared>()) - , _pFlightplan(std::make_shared(_pScenario, _pProgress)) - , _pData(std::make_shared()) - -{ -} - -SnakeDataManager::~SnakeDataManager() -{ - -} - -SnakeDataManager::SnakeDataPtr SnakeDataManager::snakeData() -{ - return _pData; -} - -bool SnakeDataManager::precondition() const -{ - - return _mArea.size() > 0 - && _sArea.size() > 0 - && _corridor.size() > 0 - && _pProgress->size() > 0; -} - -double SnakeDataManager::lineDistance() const -{ - _pFlightplan->lineDistance(); -} - -void SnakeDataManager::run() -{ - { - QMutexLocker lk(&_pData->m); - _pData->clear(); - } - - if ( !precondition() ) - return; - - if ( _mArea.size() <= 0 - || _sArea.size() <= 0 - || _corridor.size() <= 0 ){ - QMutexLocker lk(&_pData->m); - _pData->errorMessage = "At least one area invald. Size < 1."; - } - - auto origin = _mArea.front(); - // Measurement area update necessary. - if ( _mArea.size() != _pScenario->measurementArea().outer().size() ){ - { - QMutexLocker lk(&_pData->m); - _pData->ENUorigin = origin; - } - for (auto geoVertex : _mArea){ - snake::BoostPoint p; - snake::toENU(origin, geoVertex, p); - _pScenario->measurementArea().outer().push_back(p); - } - } - - // Service area update necessary. - if ( _sArea.size() != _pScenario->serviceArea().outer().size() ){ - for (auto geoVertex : _sArea){ - snake::BoostPoint p; - snake::toENU(origin, geoVertex, p); - _pScenario->serviceArea().outer().push_back(p); - } - } - - // Service area update necessary. - if ( _corridor.size() != _pScenario->corridor().outer().size() ){ - for (auto geoVertex : _corridor){ - snake::BoostPoint p; - snake::toENU(origin, geoVertex, p); - _pScenario->corridor().outer().push_back(p); - } - } - - if ( !_pScenario->update() ){ - { - QMutexLocker lk(&_pData->m); - for (auto c : _pScenario->errorString){ - _pData->errorMessage.push_back(QChar(c)); - } - } - return; - } - - // Asynchronously update flightplan. - auto future = std::async(std::bind(&snake::Flightplan::update, _pFlightplan)); - - // Continue with storing scenario data in the mean time. - { - QMutexLocker lk(&_pData->m); - // Get tiles. - const auto &tiles = _pScenario->tiles(); - const auto ¢erPoints = _pScenario->tileCenterPoints(); - for ( unsigned int i=0; i < tiles.size(); ++i ) { - const auto &tile = tiles[i]; - GeoTile geoTile; - Polygon2D enuTile; - for ( size_t i = tile.outer().size(); i < tile.outer().size()-1; ++i) { - auto &p = tile.outer()[i]; - QPointF enuVertex(p.get<0>(), p.get<1>(), 0.0); - QGeoCoordinate geoVertex; - snake::fromENU(origin, *it, geoVertex); - enuTile.polygon().push_back(enuVertex); - geoTile.push_back(QVertex); - } - const auto &boostPoint = centerPoints[i]; - QPointF enuVertex(boostPoint.get<0>(), boostPoint.get<1>(), 0.0); - QGeoCoordinate geoVertex; - snake::fromENU(origin, enuVertex, geoVertex); - geoTile.setCenter(geoVertex); - _pData->tiles.polygons().push_back(geoTile); - _pData->tileCenterPoints.push_back(QVariant::fromValue(geoVertex)); - _pData->tilesENU.polygons().push_back(enuTile); - _pData->tileCenterPointsENU.push_back(enuVertex); - } - } - - future.wait(); - // Trying to generate flight plan. - if ( !future.get() ){ - // error - QMutexLocker lk(&_pData->m); - for (auto c : _pFlightplan->errorString){ - _pData->errorMessage.push_back(QChar(c)); - } - } else { - //success!!! - QMutexLocker lk(&_pData->m); - // Store waypoints. - for (auto boostVertex : _pFlightplan->waypoints()){ - QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>(), 0.0}; - QGeoCoordinate geoVertex; - snake::fromENU(origin, enuVertex, geoVertex); - _pData->waypointsENU.push_back(enuVertex); - _pData->waypoints.push_back(geoVertex); - } - // Store arrival path. - for (auto boostVertex : _pFlightplan->arrivalPath()){ - QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>(), 0.0}; - QGeoCoordinate geoVertex; - snake::fromENU(origin, enuVertex, geoVertex); - _pData->arrivalPathENU.push_back(enuVertex); - _pData->arrivalPath.push_back(geoVertex); - } - // Store return path. - for (auto boostVertex : _pFlightplan->returnPath()){ - QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>(), 0.0}; - QGeoCoordinate geoVertex; - snake::fromENU(origin, enuVertex, geoVertex); - _pData->returnPathENU.push_back(enuVertex); - _pData->returnPath.push_back(geoVertex); - } - } -} - -double SnakeDataManager::minTransectLength() const -{ - return _pFlightplan->minTransectLength(); -} - -void SnakeDataManager::setMinTransectLength(double minTransectLength) -{ - _pFlightplan->setMinTransectLength(minTransectLength); -} - -double SnakeDataManager::minTileArea() const -{ - return _pScenario->minTileArea(); -} - -void SnakeDataManager::setLineDistance(double lineDistance) -{ - _pFlightplan->setLineDistance(lineDistance); -} - -double SnakeDataManager::tileWidth() const -{ - return _pScenario->tileWidth(); -} - -void SnakeDataManager::setTileWidth(double tileWidth) -{ - _pScenario->setTileWidth(tileWidth); -} - -double SnakeDataManager::tileHeight() const -{ - return _pScenario->_tileHeight(); -} - -void SnakeDataManager::setTileHeight(double tileHeight) -{ - _pScenario->setTileHeight(tileHeight); -} - -void SnakeDataManager::setMinTileArea(double minTileArea) -{ - _pScenario->setMinTileArea(minTileArea); -} - -SnakeData::SnakeData() -{ - -} - -void SnakeData::clear() -{ - this->waypoints.clear(); - this->arrivalPath.clear(); - this->returnPath.clear(); - this->tiles.polygons().clear(); - this->tilesLocal.polygons().clear(); - this->tileCenterPoints.clear(); - this->waypointsENU.clear(); - this->arrivalPathENU.clear(); - this->returnPathENU.clear(); - this->tilesENU.polygons().clear(); - this->tileCenterPoints.clear(); - this->errorMessage = ""; -} - -void SnakeDataManager::setCorridor(const QList &corridor) -{ - _corridor.clear(); - _pScenario->corridor().clear(); - for (auto vertex : corridor) { - _corridor.push_back(vertex); - } -} - -void SnakeDataManager::setProgress(const QList &progress) -{ - _pProgress->clear(); - for (auto p : progress) { - assert(p >= -1 && p <= 100); - _pProgress->push_back(p); - } -} - -void SnakeDataManager::setServiceArea(const QList &serviceArea) -{ - _sArea.clear(); - _pScenario->serviceArea().clear(); - for (auto vertex : serviceArea) { - _sArea.push_back(vertex); - } -} - -void SnakeDataManager::setMeasurementArea(const QList &measurementArea) -{ - _mArea.clear(); - _pScenario->measurementArea().clear(); - for (auto vertex : measurementArea) { - _mArea.push_back(vertex); - } -} diff --git a/src/Wima/Snake/SnakeWorker.h b/src/Wima/Snake/SnakeWorker.h deleted file mode 100644 index 9377815423f42e3ade3caf87e8fb2f64303d9822..0000000000000000000000000000000000000000 --- a/src/Wima/Snake/SnakeWorker.h +++ /dev/null @@ -1,103 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "GeoPolygonArray.h" -#include "PolygonArray.h" -#include "SnakeWorker.h" -#include "QNemoProgress.h" - - -typedef QList QVariantList; - -namespace snake { - class Scenario; - class Flightplan; -} - -struct SnakeData{ - SnakeData(); - - QVector waypoints; - QVector arrivalPath; - QVector returnPath; - QGeoCoordinate ENUorigin; - QVector waypointsENU; - QVector arrivalPathENU; - QVector returnPathENU; - GeoPolygonArray tiles; - QVariantList tileCenterPoints; - PolygonArray tilesENU; - QVector tileCenterPointsENU; - QString errorMessage; - mutable QMutex m; - - void clear(); -}; - -class SnakeDataManager : public QThread{ - Q_OBJECT - -public: - - using ProgressPtr = std::shared_ptr>; - using ScenarioPtr = std::shared_ptr; - using FlightPlanPtr = std::shared_ptr; - using SnakeDataPtr = std::shared_ptr; - - SnakeDataManager(QObject *parent = nullptr); - ~SnakeDataManager() override; - - - void setMeasurementArea (const QList &measurementArea); - void setServiceArea (const QList &serviceArea); - void setCorridor (const QList &corridor); - - SnakeDataPtr snakeData(); - - double lineDistance() const; - void setLineDistance(double lineDistance); - - double minTransectLength() const; - void setMinTransectLength(double minTransectLength); - - double minTileArea() const; - void setMinTileArea(double minTileArea); - - double tileHeight() const; - void setTileHeight(double tileHeight); - - double tileWidth() const; - void setTileWidth(double tileWidth); - -protected: - void run() override; - -private: - bool precondition() const; - - std::vector _mArea; - std::vector _sArea; - std::vector _corridor; - ScenarioPtr _pScenario; - ProgressPtr _pProgress; - FlightPlanPtr _pFlightplan; - SnakeDataPtr _pData; -}; - - - - - - - - - diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index c3bc50d6201ea12dbf7d59a2f7ce833770b33193..c04864a3c2ea2d1780c2e38e7a9bfa7f15767b19 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -90,22 +90,7 @@ WimaController::WimaController(QObject *parent) , _snakeTicker (EVENT_TIMER_INTERVAL, 200 /*ms*/) , _nemoTimeoutTicker (EVENT_TIMER_INTERVAL, 5000 /*ms*/) , _topicServiceSetupDone (false) -{ - - // 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 { - qgcApp()->showMessage("ROS Bridge connection string invalid: " + connectionString); - this->_pRosBridge.reset(new ros_bridge::ROSBridge("localhost:9090")); - } - }; - setConnectionString(); - connect(wimaSettings->rosbridgeConnectionString(), &SettingsFact::rawValueChanged, setConnectionString); +{ // Set up facts. _showAllMissionItems.setRawValue(true); @@ -138,9 +123,9 @@ WimaController::WimaController(QObject *parent) _eventTimer.start(EVENT_TIMER_INTERVAL); // Snake Worker Thread. - connect(&_snakeWorker, &SnakeWorker::finished, this, &WimaController::_snakeStoreWorkerResults); + connect(&_snakeDataManager, &SnakeDataManager::finished, this, &WimaController::_snakeStoreWorkerResults); connect(this, &WimaController::nemoProgressChanged, this, &WimaController::_initStartSnakeWorker); - connect(this, &QObject::destroyed, &this->_snakeWorker, &SnakeWorker::quit); + connect(this, &QObject::destroyed, &this->_snakeDataManager, &SnakeWorker::quit); // Snake. connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_initStartSnakeWorker); @@ -899,7 +884,7 @@ void WimaController::_snakeStoreWorkerResults() auto start = std::chrono::high_resolution_clock::now(); _snakeManager.clear(); - const auto &r = _snakeWorker.getResult(); + const auto &r = _snakeDataManager.getResult(); if (!r.success) { //qgcApp()->showMessage(r.errorMessage); return; @@ -936,27 +921,27 @@ void WimaController::_initStartSnakeWorker() return; // Stop worker thread if running. - if ( _snakeWorker.isRunning() ) { - _snakeWorker.quit(); + if ( _snakeDataManager.isRunning() ) { + _snakeDataManager.quit(); } // Initialize _snakeWorker. - _snakeWorker.setProgress( + _snakeDataManager.setProgress( _nemoProgress.progress()); - _snakeWorker.setLineDistance( + _snakeDataManager.setLineDistance( _snakeLineDistance.rawValue().toDouble()); - _snakeWorker.setMinTransectLength( + _snakeDataManager.setMinTransectLength( _snakeMinTransectLength.rawValue().toDouble()); - _snakeWorker.setTileHeight( + _snakeDataManager.setTileHeight( _snakeTileHeight.rawValue().toDouble()); - _snakeWorker.setTileWidth( + _snakeDataManager.setTileWidth( _snakeTileWidth.rawValue().toDouble()); - _snakeWorker.setMinTileArea( + _snakeDataManager.setMinTileArea( _snakeMinTileArea.rawValue().toDouble()); _setSnakeCalcInProgress(true); // Start worker thread. - _snakeWorker.start(); + _snakeDataManager.start(); } void WimaController::_switchSnakeManager(QVariant variant) diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index 924467b8b74704d1eaddde597b8d7997590c72f0..6eff5b91226666163636b41d67b2af4b09423de3 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -24,16 +24,13 @@ #include "JsonHelper.h" #include "QGCApplication.h" #include "SettingsFact.h" -#include "WimaSettings.h" #include "SettingsManager.h" #include "snake.h" -#include "Snake/SnakeWorker.h" -#include "Snake/GeoPolygonArray.h" -#include "Snake/PolygonArray.h" +#include "Snake/SnakeDataManager.h" +#include "Snake/SnakeTiles.h" +#include "Snake/SnakeTilesLocal.h" #include "Geometry/GeoPoint3D.h" -#include "Snake/QNemoProgress.h" -#include "Snake/QNemoHeartbeat.h" #include "ros_bridge/include/ros_bridge.h" @@ -52,11 +49,7 @@ typedef std::unique_ptr JsonDocUPtr; class WimaController : public QObject { Q_OBJECT - enum FileType {WimaFile, PlanFile}; - - typedef QScopedPointer ROSBridgePtr; - public: @@ -395,18 +388,9 @@ private: double _measurementPathLength; // the lenght of the phase in meters // Snake - bool _snakeCalcInProgress; - SnakeDataManager _snakeWorker; - GeoPoint _snakeOrigin; - GeoPolygonArray _snakeTiles; // tiles - PolygonArray _snakeTilesLocal; // tiles local coordinate system - QVariantList _snakeTileCenterPoints; - QNemoProgress _nemoProgress; // measurement progress - QNemoHeartbeat _nemoHeartbeat; // measurement progress + SnakeDataManager _snakeDataManager; int _fallbackStatus; - ROSBridgePtr _pRosBridge; static StatusMap _nemoStatusMap; - bool _topicServiceSetupDone; // Periodic tasks. QTimer _eventTimer;