Commit f060c3b7 authored by Valentin Platzgummer's avatar Valentin Platzgummer

wima controller mod

parent af4b4687
This diff is collapsed.
......@@ -51,6 +51,9 @@ public:
}
QList<QObject *> *objectList() { return &_objectList; }
bool operator==(const QmlObjectListModel &other);
bool operator!=(const QmlObjectListModel &other);
/// Calls deleteLater on all items and this itself.
void deleteListAndContents();
......
......@@ -9,6 +9,38 @@
#define SNAKE_MAX_TILES 1000
#endif
TileData::TileData() : tiles(this) {}
TileData::~TileData() { tiles.clearAndDeleteContents(); }
TileData &TileData::operator=(const TileData &other) {
this->tiles.clearAndDeleteContents();
for (std::size_t i = 0; i < std::size_t(other.tiles.count()); ++i) {
const auto *obj = other.tiles.get(i);
const auto *tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
this->tiles.append(new SnakeTile(*tile, this));
} else {
qWarning("TileData::operator=: nullptr");
}
}
this->tileCenterPoints = other.tileCenterPoints;
return *this;
}
void TileData::clear() {
this->tiles.clearAndDeleteContents();
this->tileCenterPoints.clear();
}
size_t TileData::size() const {
if (tiles.count() == tileCenterPoints.size()) {
return tiles.count();
} else {
return 0;
}
}
const char *WimaMeasurementArea::settingsGroup = "MeasurementArea";
const char *WimaMeasurementArea::tileHeightName = "TileHeight";
const char *WimaMeasurementArea::tileWidthName = "TileWidth";
......@@ -39,8 +71,7 @@ WimaMeasurementArea::WimaMeasurementArea(QObject *parent)
this /* QObject parent */)),
_showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName],
this /* QObject parent */)),
_pTiles(new QmlObjectListModel(), &tileDeleter), _calculating(false),
_polygonValid(false) {
_calculating(false) {
init();
}
......@@ -64,8 +95,7 @@ WimaMeasurementArea::WimaMeasurementArea(const WimaMeasurementArea &other,
this /* QObject parent */)),
_showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName],
this /* QObject parent */)),
_pTiles(new QmlObjectListModel(), &tileDeleter), _calculating(false),
_polygonValid(false) {
_calculating(false) {
init();
}
......@@ -81,9 +111,7 @@ operator=(const WimaMeasurementArea &other) {
return *this;
}
WimaMeasurementArea::~WimaMeasurementArea() {
this->_pTiles->clearAndDeleteContents();
}
WimaMeasurementArea::~WimaMeasurementArea() {}
QString WimaMeasurementArea::mapVisualQML() const {
return "WimaMeasurementAreaMapVisual.qml";
......@@ -105,10 +133,20 @@ Fact *WimaMeasurementArea::minTransectLength() { return &_minTransectLength; }
Fact *WimaMeasurementArea::showTiles() { return &_showTiles; }
QmlObjectListModel *WimaMeasurementArea::tiles() { return this->_pTiles.get(); }
QmlObjectListModel *WimaMeasurementArea::tiles() {
return &this->_tileData.tiles;
}
const QmlObjectListModel *WimaMeasurementArea::tiles() const {
return this->_pTiles.get();
return &this->_tileData.tiles;
}
const QVariantList &WimaMeasurementArea::tileCenterPoints() const {
return this->_tileData.tileCenterPoints;
}
const TileData &WimaMeasurementArea::tileData() const {
return this->_tileData;
}
int WimaMeasurementArea::maxTiles() const { return SNAKE_MAX_TILES; }
......@@ -181,8 +219,8 @@ bool WimaMeasurementArea::loadFromJson(const QJsonObject &json,
}
//!
//! \brief WimaMeasurementArea::doUpdate
//! \pre WimaMeasurementArea::deferUpdate must be called first, don't call this
//! function directly!
//! \pre WimaMeasurementArea::deferUpdate must be called first, don't call
//! this function directly!
void WimaMeasurementArea::doUpdate() {
using namespace snake;
using namespace boost::units;
......@@ -198,14 +236,10 @@ void WimaMeasurementArea::doUpdate() {
long(std::ceil(estNumTiles.value())) <= SNAKE_MAX_TILES &&
this->count() >= 3 && this->isSimplePolygon()) {
this->_calculating = true;
if (!this->_polygonValid) {
this->_polygon = this->coordinateList();
for (auto &v : this->_polygon) {
v.setAltitude(0);
}
this->_polygonValid = true;
auto polygon = this->coordinateList();
for (auto &v : polygon) {
v.setAltitude(0);
}
const auto &polygon = this->_polygon;
const auto minArea =
this->_minTileArea.rawValue().toDouble() * si::meter * si::meter;
auto *th = this->thread();
......@@ -213,35 +247,44 @@ void WimaMeasurementArea::doUpdate() {
#ifdef SNAKE_SHOW_TIME
auto start = std::chrono::high_resolution_clock::now();
#endif
TilesPtr pTiles(new QmlObjectListModel(), &tileDeleter);
DataPtr pData(new TileData());
// Convert to ENU system.
QGeoCoordinate origin = polygon.first();
BoostPolygon polygonENU;
areaToEnu(origin, polygon, polygonENU);
std::vector<BoostPolygon> 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(pTiles.get());
auto geoTile = new SnakeTile(pData.get());
for (const auto &v : t.outer()) {
QGeoCoordinate geoVertex;
fromENU(origin, v, geoVertex);
geoTile->push_back(geoVertex);
}
pTiles->append(geoTile);
pData->tiles.append(geoTile);
// Calculate center.
snake::BoostPoint center;
snake::polygonCenter(t, center);
QGeoCoordinate geoCenter;
fromENU(origin, center, geoCenter);
pData->tileCenterPoints.append(QVariant::fromValue(geoCenter));
}
}
pTiles->moveToThread(th);
pData->moveToThread(th);
#ifdef SNAKE_SHOW_TIME
qDebug()
<< "WimaMeasurementArea::doUpdate concurrent update execution time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start)
.count()
<< " ms";
qDebug() << "WimaMeasurementArea::doUpdate concurrent update execution "
"time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start)
.count()
<< " ms";
#endif
return pTiles;
return pData;
}); // QtConcurrent::run()
this->_watcher.setFuture(future);
......@@ -259,8 +302,8 @@ void WimaMeasurementArea::deferUpdate() {
if (this->_timer.isActive()) {
this->_timer.stop();
}
if (this->_pTiles->count() > 0) {
this->_pTiles->clearAndDeleteContents();
if (this->_tileData.size() > 0) {
this->_tileData.clear();
emit this->tilesChanged();
}
this->_timer.start(100);
......@@ -270,10 +313,10 @@ void WimaMeasurementArea::storeTiles() {
#ifdef SNAKE_SHOW_TIME
auto start = std::chrono::high_resolution_clock::now();
#endif
this->_pTiles = this->_watcher.result();
this->_tileData = *this->_watcher.result();
this->_calculating = false;
emit this
->tilesChanged(); // This is expensive. Drawing tiles is expensive too.
// This is expensive. Drawing tiles is expensive too.
emit this->tilesChanged();
#ifdef SNAKE_SHOW_TIME
qDebug() << "WimaMeasurementArea::storeTiles() execution time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
......@@ -293,8 +336,6 @@ void WimaMeasurementArea::init() {
&WimaMeasurementArea::deferUpdate);
connect(this, &WimaArea::pathChanged, this,
&WimaMeasurementArea::deferUpdate);
connect(this, &WimaArea::pathChanged,
[this] { this->_polygonValid = false; });
this->_timer.setSingleShot(true);
connect(&this->_timer, &QTimer::timeout, this,
&WimaMeasurementArea::doUpdate);
......
......@@ -8,6 +8,20 @@
#include "SettingsFact.h"
class TileData : public QObject {
public:
QmlObjectListModel tiles;
QVariantList tileCenterPoints;
TileData();
~TileData();
TileData &operator=(const TileData &other);
void clear();
std::size_t size() const;
};
class WimaMeasurementArea : public WimaArea {
Q_OBJECT
public:
......@@ -38,6 +52,8 @@ public:
Fact *showTiles();
QmlObjectListModel *tiles();
const QmlObjectListModel *tiles() const;
const QVariantList &tileCenterPoints() const; // List of QGeoCoordinate
const TileData &tileData() const;
int maxTiles() const;
bool ready() const;
......@@ -86,10 +102,8 @@ private:
// Tile stuff.
QTimer _timer;
using TilesPtr = std::shared_ptr<QmlObjectListModel>;
TilesPtr _pTiles;
QList<QGeoCoordinate> _polygon;
QFutureWatcher<TilesPtr> _watcher;
using DataPtr = std::shared_ptr<TileData>;
TileData _tileData;
QFutureWatcher<DataPtr> _watcher;
bool _calculating;
bool _polygonValid;
};
......@@ -44,33 +44,45 @@ operator=(const WimaMeasurementArea &other) {
QString WimaMeasurementAreaData::type() const { return this->typeString; }
QmlObjectListModel *WimaMeasurementAreaData::tiles() {
return &this->_tileData.tiles;
}
const QmlObjectListModel *WimaMeasurementAreaData::tiles() const {
return &this->_tileData.tiles;
}
const QVariantList &WimaMeasurementAreaData::tileCenterPoints() const {
return this->_tileData.tileCenterPoints;
}
QVariantList &WimaMeasurementAreaData::tileCenterPoints() {
return this->_tileData.tileCenterPoints;
}
const TileData &WimaMeasurementAreaData::tileData() const {
return this->tileData();
}
TileData &WimaMeasurementAreaData::tileData() { return this->tileData(); }
const QVector<int> &WimaMeasurementAreaData::progress() const {
return this->_progress;
}
QVector<int> &WimaMeasurementAreaData::progress() { return this->_progress; }
void WimaMeasurementAreaData::assign(const WimaMeasurementAreaData &other) {
WimaAreaData::assign(other);
this->tiles.clearAndDeleteContents();
for (std::size_t i = 0; i < std::size_t(other.tiles.count()); ++i) {
const auto *obj = other.tiles.get(i);
const auto *tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
this->tiles.append(new SnakeTile(*tile, this));
} else {
qWarning() << "WimaMeasurementAreaData::assign(): type cast failed.";
}
}
this->_tileData = other._tileData;
this->_progress = other._progress;
}
void WimaMeasurementAreaData::assign(const WimaMeasurementArea &other) {
WimaAreaData::assign(other);
this->tiles.clearAndDeleteContents();
if (other.ready()) {
for (std::size_t i = 0; i < std::size_t(other.tiles()->count()); ++i) {
const auto *obj = other.tiles()->get(i);
const auto *tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
this->tiles.append(new SnakeTile(*tile, this));
} else {
qWarning() << "WimaMeasurementAreaData::assign(): type cast failed.";
}
}
this->_tileData = other.tileData();
qWarning() << "WimaMeasurementAreaData: add progress copy here.";
} else {
qWarning()
<< "WimaMeasurementAreaData::assign(): WimaMeasurementArea not ready.";
......
......@@ -23,17 +23,22 @@ public:
return new WimaMeasurementAreaData(*this);
}
static const char *typeString;
signals:
QmlObjectListModel *tiles();
const QmlObjectListModel *tiles() const;
const QVariantList &tileCenterPoints() const;
QVariantList &tileCenterPoints();
const TileData &tileData() const;
TileData &tileData();
const QVector<int> &progress() const;
QVector<int> &progress();
public slots:
static const char *typeString;
protected:
void assign(const WimaMeasurementAreaData &other);
void assign(const WimaMeasurementArea &other);
private:
// see WimaMeasurementArea.h for explanation
QmlObjectListModel tiles;
TileData _tileData;
QVector<int> _progress;
};
......@@ -12,6 +12,9 @@
#include "QNemoHeartbeat.h"
#include "QNemoProgress.h"
#include "Wima/Geometry/WimaMeasurementArea.h"
#include "Wima/Snake/SnakeTile.h"
#include "Wima/Snake/snake.h"
#include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
#include "ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h"
......@@ -39,8 +42,9 @@ public:
void start();
void stop();
void setTilesENU(const SnakeTilesLocal &tilesENU);
void setENUOrigin(const QGeoCoordinate &ENUOrigin);
void setTileData(const TileData &tileData);
bool hasTileData(const TileData &tileData) const;
NemoInterface::NemoStatus status();
QVector<int> progress();
......@@ -69,6 +73,9 @@ private:
TimePoint nextTimeout;
mutable std::shared_timed_mutex heartbeatMutex;
// Not protected data.
TileData tileData;
// Internals
bool running;
std::atomic_bool topicServiceSetupDone;
......@@ -113,24 +120,49 @@ void NemoInterface::Impl::start() { this->running = true; }
void NemoInterface::Impl::stop() { this->running = false; }
void NemoInterface::Impl::setTilesENU(const SnakeTilesLocal &tilesENU) {
UniqueLock lk(this->tilesENUMutex);
this->tilesENU = tilesENU;
lk.unlock();
if (this->running && this->topicServiceSetupDone) {
lk.lock();
this->publishTilesENU();
void NemoInterface::Impl::setTileData(const TileData &tileData) {
this->tileData = tileData;
if (tileData.tiles.count() > 0) {
std::lock(this->ENUOriginMutex, this->tilesENUMutex);
UniqueLock lk1(this->ENUOriginMutex, std::adopt_lock);
UniqueLock lk2(this->tilesENUMutex, std::adopt_lock);
const auto *obj = tileData.tiles.get(0);
const auto *tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
if (tile->coordinateList().size() > 0) {
this->ENUOrigin = tile->coordinateList().first();
const auto &origin = this->ENUOrigin;
this->tilesENU.polygons().clear();
bool error = false;
for (std::size_t i = 0; i < tileData.tiles.count(); ++i) {
*obj = tileData.tiles.get(i);
*tile = qobject_cast<const SnakeTile *>(obj);
if (tile != nullptr) {
snake::BoostPolygon tileENU;
snake::areaToEnu(origin, tile->coordinateList(), tileENU);
this->tilesENU.polygons().push_back(std::move(tileENU));
} else {
qWarning() << "NemoInterface::Impl::setTileData(): nullptr.";
error = true;
break;
}
}
if (!error && this->running && this->topicServiceSetupDone) {
this->publishENUOrigin();
this->publishTilesENU();
} else {
qWarning() << "NemoInterface::Impl::setTileData(): first tile empty.";
}
} else {
qWarning() << "NemoInterface::Impl::setTileData(): nullptr.";
}
}
}
}
void NemoInterface::Impl::setENUOrigin(const QGeoCoordinate &ENUOrigin) {
UniqueLock lk(this->ENUOriginMutex);
this->ENUOrigin = ENUOrigin;
lk.unlock();
if (this->running && this->topicServiceSetupDone) {
lk.lock();
this->publishENUOrigin();
}
bool NemoInterface::Impl::hasTileData(const TileData &tileData) const {
return this->tileData = tileData;
}
NemoInterface::NemoStatus NemoInterface::Impl::status() {
......@@ -341,12 +373,12 @@ void NemoInterface::start() { this->pImpl->start(); }
void NemoInterface::stop() { this->pImpl->stop(); }
void NemoInterface::setTilesENU(const SnakeTilesLocal &tilesENU) {
this->pImpl->setTilesENU(tilesENU);
void NemoInterface::publishTileData(const TileData &tileData) {
this->pImpl->setTileData(tileData);
}
void NemoInterface::setENUOrigin(const QGeoCoordinate &ENUOrigin) {
this->pImpl->setENUOrigin(ENUOrigin);
bool NemoInterface::hasTileData(const TileData &tileData) const {
return this->pImpl->hasTileData(tileData);
}
NemoInterface::NemoStatus NemoInterface::status() const {
......
......@@ -7,6 +7,8 @@
#include <memory>
class TileData;
class NemoInterface : public QObject {
Q_OBJECT
class Impl;
......@@ -26,8 +28,8 @@ public:
void start();
void stop();
void setTilesENU(const SnakeTilesLocal &tilesENU);
void setENUOrigin(const QGeoCoordinate &ENUOrigin);
void publishTileData(const TileData &tileData);
bool hasTileData(const TileData &tileData) const;
NemoStatus status() const;
QVector<int> progress() const;
......
This diff is collapsed.
......@@ -6,31 +6,21 @@
#include "QGCMapPolygon.h"
#include "QmlObjectListModel.h"
#include "Geometry/WimaArea.h"
#include "Geometry/WimaCorridor.h"
//#include "Geometry/WimaArea.h"
//#include "Geometry/WimaCorridor.h"
#include "Geometry/WimaCorridorData.h"
#include "Geometry/WimaMeasurementArea.h"
//#include "Geometry/WimaMeasurementArea.h"
#include "Geometry/WimaMeasurementAreaData.h"
#include "Geometry/WimaServiceArea.h"
//#include "Geometry/WimaServiceArea.h"
#include "Geometry/WimaServiceAreaData.h"
#include "WimaPlanData.h"
#include "JsonHelper.h"
#include "MissionController.h"
#include "MissionSettingsItem.h"
#include "PlanMasterController.h"
#include "QGCApplication.h"
#include "SettingsFact.h"
#include "SettingsManager.h"
#include "SimpleMissionItem.h"
#include "SurveyComplexItem.h"
#include "Geometry/GeoPoint3D.h"
#include "RoutingThread.h"
#include "Snake/NemoInterface.h"
#include "Snake/SnakeThread.h"
#include "Snake/SnakeTiles.h"
#include "Snake/SnakeTilesLocal.h"
#include "WaypointManager/DefaultManager.h"
#include "WaypointManager/RTLManager.h"
......@@ -89,15 +79,10 @@ public:
nemoStatusStringChanged)
Q_PROPERTY(bool snakeCalcInProgress READ snakeCalcInProgress NOTIFY
snakeCalcInProgressChanged)
Q_PROPERTY(Fact *snakeTileWidth READ snakeTileWidth CONSTANT)
Q_PROPERTY(Fact *snakeTileHeight READ snakeTileHeight CONSTANT)
Q_PROPERTY(Fact *snakeMinTileArea READ snakeMinTileArea CONSTANT)
Q_PROPERTY(Fact *snakeLineDistance READ snakeLineDistance CONSTANT)
Q_PROPERTY(Fact *snakeMinTransectLength READ snakeMinTransectLength CONSTANT)
Q_PROPERTY(
QmlObjectListModel *snakeTiles READ snakeTiles NOTIFY snakeTilesChanged)
Q_PROPERTY(QVariantList snakeTileCenterPoints READ snakeTileCenterPoints
NOTIFY snakeTileCenterPointsChanged)
NOTIFY snakeTilesChanged)
Q_PROPERTY(
QVector<int> nemoProgress READ nemoProgress NOTIFY nemoProgressChanged)
......@@ -125,11 +110,6 @@ public:
Fact *altitude(void);
// Snake settings facts.
Fact *enableSnake(void) { return &_enableSnake; }
Fact *snakeTileWidth(void) { return &_snakeTileWidth; }
Fact *snakeTileHeight(void) { return &_snakeTileHeight; }
Fact *snakeMinTileArea(void) { return &_snakeMinTileArea; }
Fact *snakeLineDistance(void) { return &_snakeLineDistance; }
Fact *snakeMinTransectLength(void) { return &_snakeMinTransectLength; }
// Snake data.
QmlObjectListModel *snakeTiles(void);
QVariantList snakeTileCenterPoints(void);
......@@ -179,9 +159,6 @@ public:
static const char *flightSpeedName;
static const char *arrivalReturnSpeedName;
static const char *altitudeName;
static const char *snakeTileWidthName;
static const char *snakeTileHeightName;
static const char *snakeMinTileAreaName;
static const char *snakeLineDistanceName;
static const char *snakeMinTransectLengthName;
......@@ -207,7 +184,6 @@ signals:
// Snake.
void snakeCalcInProgressChanged(void);
void snakeTilesChanged(void);
void snakeTileCenterPointsChanged(void);
void nemoProgressChanged(void);
void nemoStatusChanged(void);
void nemoStatusStringChanged(void);
......@@ -240,9 +216,9 @@ private slots:
void _threadFinishedHandler();
void _switchWaypointManager(WaypointManager::ManagerBase &manager);
void _switchToSnakeWaypointManager(QVariant variant);
void _switchThreadObject(SnakeThread &thread);
void _progressChangedHandler();
void _enableSnakeChangedHandler();
void _updateRoute();
// Periodic tasks.
void _eventTimerHandler(void);
......@@ -293,11 +269,6 @@ private:
SettingsFact _arrivalReturnSpeed; // arrival and return path speed
SettingsFact _altitude; // mission altitude
SettingsFact _enableSnake; // Enable Snake (see snake.h)
SettingsFact _snakeTileWidth;
SettingsFact _snakeTileHeight;
SettingsFact _snakeMinTileArea;
SettingsFact _snakeLineDistance;
SettingsFact _snakeMinTransectLength;
// Smart RTL.
QTimer _smartRTLTimer;
......@@ -308,13 +279,10 @@ private:
// Snake
QList<QList<QGeoCoordinate>> _rawTransects;
QmlObjectListModel tiles;
SnakeThread _snakeThread; // Snake Data Manager
SnakeThread _emptyThread;
SnakeThread *_currentThread;
NemoInterface _nemoInterface;
using StatusMap = std::map<int, QString>;
static StatusMap _nemoStatusMap;
RoutingThread _routingThread;
// Periodic tasks.
QTimer _eventTimer;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment