Skip to content
GitLab
Commit f060c3b7 authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files

wima controller mod

parent af4b4687
Show whitespace changes
Inline Side-by-side
src/QmlControls/QmlObjectListModel.cc
View file @ f060c3b7
......@@ -7,7 +7,6 @@
*
****************************************************************************/
/// @file
/// @author Don Gagne <don@thegagnes.com>
......@@ -19,28 +18,18 @@
const int QmlObjectListModel::ObjectRole = Qt::UserRole;
const int QmlObjectListModel::TextRole = Qt::UserRole + 1;
QmlObjectListModel::QmlObjectListModel(QObject* parent)
: QAbstractListModel(parent)
, _dirty(false)
, _skipDirtyFirstItem(false)
{
}
QmlObjectListModel::QmlObjectListModel(QObject *parent)
: QAbstractListModel(parent), _dirty(false), _skipDirtyFirstItem(false) {}
QmlObjectListModel::~QmlObjectListModel()
{
}
QmlObjectListModel::~QmlObjectListModel() {}
int QmlObjectListModel::rowCount(const QModelIndex& parent) const
{
int QmlObjectListModel::rowCount(const QModelIndex &parent) const {
Q_UNUSED(parent);
return _objectList.count();
}
QVariant QmlObjectListModel::data(const QModelIndex &index, int role) const
{
QVariant QmlObjectListModel::data(const QModelIndex &index, int role) const {
if (!index.isValid()) {
return QVariant();
}
......@@ -58,8 +47,7 @@ QVariant QmlObjectListModel::data(const QModelIndex &index, int role) const
}
}
QHash<int, QByteArray> QmlObjectListModel::roleNames(void) const
{
QHash<int, QByteArray> QmlObjectListModel::roleNames(void) const {
QHash<int, QByteArray> hash;
hash[ObjectRole] = "object";
......@@ -68,10 +56,10 @@ QHash<int, QByteArray> QmlObjectListModel::roleNames(void) const
return hash;
}
bool QmlObjectListModel::setData(const QModelIndex& index, const QVariant& value, int role)
{
bool QmlObjectListModel::setData(const QModelIndex &index,
const QVariant &value, int role) {
if (index.isValid() && role == ObjectRole) {
_objectList.replace(index.row(), value.value<QObject*>());
_objectList.replace(index.row(), value.value<QObject *>());
emit dataChanged(index, index);
return true;
}
......@@ -79,12 +67,13 @@ bool QmlObjectListModel::setData(const QModelIndex& index, const QVariant& value
return false;
}
bool QmlObjectListModel::insertRows(int position, int rows, const QModelIndex& parent)
{
bool QmlObjectListModel::insertRows(int position, int rows,
const QModelIndex &parent) {
Q_UNUSED(parent);
if (position < 0 || position > _objectList.count() + 1) {
qWarning() << "Invalid position position:count" << position << _objectList.count();
qWarning() << "Invalid position position:count" << position
<< _objectList.count();
}
beginInsertRows(QModelIndex(), position, position + rows - 1);
......@@ -95,18 +84,20 @@ bool QmlObjectListModel::insertRows(int position, int rows, const QModelIndex& p
return true;
}
bool QmlObjectListModel::removeRows(int position, int rows, const QModelIndex& parent)
{
bool QmlObjectListModel::removeRows(int position, int rows,
const QModelIndex &parent) {
Q_UNUSED(parent);
if (position < 0 || position >= _objectList.count()) {
qWarning() << "Invalid position position:count" << position << _objectList.count();
qWarning() << "Invalid position position:count" << position
<< _objectList.count();
} else if (position + rows > _objectList.count()) {
qWarning() << "Invalid rows position:rows:count" << position << rows << _objectList.count();
qWarning() << "Invalid rows position:rows:count" << position << rows
<< _objectList.count();
}
beginRemoveRows(QModelIndex(), position, position + rows - 1);
for (int row=0; row<rows; row++) {
for (int row = 0; row < rows; row++) {
_objectList.removeAt(position);
}
endRemoveRows();
......@@ -116,37 +107,52 @@ bool QmlObjectListModel::removeRows(int position, int rows, const QModelIndex& p
return true;
}
QObject* QmlObjectListModel::operator[](int index)
{
QObject *QmlObjectListModel::operator[](int index) {
if (index < 0 || index >= _objectList.count()) {
return NULL;
}
return _objectList[index];
}
const QObject* QmlObjectListModel::operator[](int index) const
{
const QObject *QmlObjectListModel::operator[](int index) const {
if (index < 0 || index >= _objectList.count()) {
return NULL;
}
return _objectList[index];
}
void QmlObjectListModel::clear()
{
bool QmlObjectListModel::operator==(const QmlObjectListModel &other) {
if (this->count() == other.count()) {
for (std::size_t i = 0; i < this->count(); ++i) {
if (this->get(i) != other.get(i)) {
return false;
}
}
return true;
} else {
return false;
}
}
bool QmlObjectListModel::operator==(const QmlObjectListModel &other) {
return !this->operator==(other);
}
void QmlObjectListModel::clear() {
while (rowCount()) {
removeAt(0);
}
}
QObject* QmlObjectListModel::removeAt(int i)
{
QObject* removedObject = _objectList[i];
if(removedObject) {
QObject *QmlObjectListModel::removeAt(int i) {
QObject *removedObject = _objectList[i];
if (removedObject) {
// Look for a dirtyChanged signal on the object
if (_objectList[i]->metaObject()->indexOfSignal(QMetaObject::normalizedSignature("dirtyChanged(bool)")) != -1) {
if (_objectList[i]->metaObject()->indexOfSignal(
QMetaObject::normalizedSignature("dirtyChanged(bool)")) != -1) {
if (!_skipDirtyFirstItem || i != 0) {
QObject::disconnect(_objectList[i], SIGNAL(dirtyChanged(bool)), this, SLOT(_childDirtyChanged(bool)));
QObject::disconnect(_objectList[i], SIGNAL(dirtyChanged(bool)), this,
SLOT(_childDirtyChanged(bool)));
}
}
}
......@@ -155,8 +161,7 @@ QObject* QmlObjectListModel::removeAt(int i)
return removedObject;
}
void QmlObjectListModel::insert(int i, QObject* object)
{
void QmlObjectListModel::insert(int i, QObject *object) {
if (i < 0 || i > _objectList.count()) {
qWarning() << "Invalid index index:count" << i << _objectList.count();
}
......@@ -164,9 +169,11 @@ void QmlObjectListModel::insert(int i, QObject* object)
QQmlEngine::setObjectOwnership(object, QQmlEngine::CppOwnership);
// Look for a dirtyChanged signal on the object
if (object->metaObject()->indexOfSignal(QMetaObject::normalizedSignature("dirtyChanged(bool)")) != -1) {
if (object->metaObject()->indexOfSignal(
QMetaObject::normalizedSignature("dirtyChanged(bool)")) != -1) {
if (!_skipDirtyFirstItem || i != 0) {
QObject::connect(object, SIGNAL(dirtyChanged(bool)), this, SLOT(_childDirtyChanged(bool)));
QObject::connect(object, SIGNAL(dirtyChanged(bool)), this,
SLOT(_childDirtyChanged(bool)));
}
}
......@@ -176,20 +183,21 @@ void QmlObjectListModel::insert(int i, QObject* object)
setDirty(true);
}
void QmlObjectListModel::insert(int i, QList<QObject*> objects)
{
void QmlObjectListModel::insert(int i, QList<QObject *> objects) {
if (i < 0 || i > _objectList.count()) {
qWarning() << "Invalid index index:count" << i << _objectList.count();
}
int j = i;
for (QObject* object: objects) {
for (QObject *object : objects) {
QQmlEngine::setObjectOwnership(object, QQmlEngine::CppOwnership);
// Look for a dirtyChanged signal on the object
if (object->metaObject()->indexOfSignal(QMetaObject::normalizedSignature("dirtyChanged(bool)")) != -1) {
if (object->metaObject()->indexOfSignal(
QMetaObject::normalizedSignature("dirtyChanged(bool)")) != -1) {
if (!_skipDirtyFirstItem || j != 0) {
QObject::connect(object, SIGNAL(dirtyChanged(bool)), this, SLOT(_childDirtyChanged(bool)));
QObject::connect(object, SIGNAL(dirtyChanged(bool)), this,
SLOT(_childDirtyChanged(bool)));
}
}
j++;
......@@ -202,18 +210,15 @@ void QmlObjectListModel::insert(int i, QList<QObject*> objects)
setDirty(true);
}
void QmlObjectListModel::append(QObject* object)
{
void QmlObjectListModel::append(QObject *object) {
insert(_objectList.count(), object);
}
void QmlObjectListModel::append(QList<QObject*> objects)
{
void QmlObjectListModel::append(QList<QObject *> objects) {
insert(_objectList.count(), objects);
}
QObjectList QmlObjectListModel::swapObjectList(const QObjectList& newlist)
{
QObjectList QmlObjectListModel::swapObjectList(const QObjectList &newlist) {
QObjectList oldlist(_objectList);
beginResetModel();
_objectList = newlist;
......@@ -222,18 +227,14 @@ QObjectList QmlObjectListModel::swapObjectList(const QObjectList& newlist)
return oldlist;
}
int QmlObjectListModel::count() const
{
return rowCount();
}
int QmlObjectListModel::count() const { return rowCount(); }
void QmlObjectListModel::setDirty(bool dirty)
{
void QmlObjectListModel::setDirty(bool dirty) {
if (_dirty != dirty) {
_dirty = dirty;
if (!dirty) {
// Need to clear dirty from all children
for(QObject* object: _objectList) {
for (QObject *object : _objectList) {
if (object->property("dirty").isValid()) {
object->setProperty("dirty", false);
}
......@@ -243,34 +244,31 @@ void QmlObjectListModel::setDirty(bool dirty)
}
}
void QmlObjectListModel::_childDirtyChanged(bool dirty)
{
void QmlObjectListModel::_childDirtyChanged(bool dirty) {
_dirty |= dirty;
// We want to emit dirtyChanged even if the actual value of _dirty didn't change. It can be a useful
// signal to know when a child has changed dirty state
// We want to emit dirtyChanged even if the actual value of _dirty didn't
// change. It can be a useful signal to know when a child has changed dirty
// state
emit dirtyChanged(_dirty);
}
void QmlObjectListModel::deleteListAndContents()
{
for (int i=0; i<_objectList.count(); i++) {
void QmlObjectListModel::deleteListAndContents() {
for (int i = 0; i < _objectList.count(); i++) {
_objectList[i]->deleteLater();
}
deleteLater();
}
void QmlObjectListModel::clearAndDeleteContents()
{
void QmlObjectListModel::clearAndDeleteContents() {
beginResetModel();
for (int i=0; i<_objectList.count(); i++) {
for (int i = 0; i < _objectList.count(); i++) {
_objectList[i]->deleteLater();
}
clear();
endResetModel();
}
void swap(QmlObjectListModel &list1, QmlObjectListModel &list2)
{
void swap(QmlObjectListModel &list1, QmlObjectListModel &list2) {
using std::swap;
swap(list1._objectList, list2._objectList);
......
src/QmlControls/QmlObjectListModel.h
View file @ f060c3b7
......@@ -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();
......
src/Wima/Geometry/WimaMeasurementArea.cc
View file @ f060c3b7
......@@ -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) {
auto polygon = this->coordinateList();
for (auto &v : polygon) {
v.setAltitude(0);
}
this->_polygonValid = true;
}
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: "
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);
......
src/Wima/Geometry/WimaMeasurementArea.h
View file @ f060c3b7
......@@ -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;
};
src/Wima/Geometry/WimaMeasurementAreaData.cc
View file @ f060c3b7
......@@ -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.";
......
src/Wima/Geometry/WimaMeasurementAreaData.h
View file @ f060c3b7
......@@ -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;
};
src/Wima/Snake/NemoInterface.cpp
View file @ f060c3b7
......@@ -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();
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 {
......
src/Wima/Snake/NemoInterface.h
View file @ f060c3b7
......@@ -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;
......
src/Wima/WimaController.cc
View file @ f060c3b7
......@@ -2,6 +2,12 @@
#include "utilities.h"
#include "MissionController.h"
#include "MissionSettingsItem.h"
#include "PlanMasterController.h"
#include "QGCApplication.h"
#include "SimpleMissionItem.h"
#include "Snake/QNemoHeartbeat.h"
#include "Snake/QNemoProgress.h"
......@@ -33,12 +39,6 @@ const char *WimaController::showCurrentMissionItemsName =
const char *WimaController::flightSpeedName = "FlightSpeed";
const char *WimaController::arrivalReturnSpeedName = "ArrivalReturnSpeed";
const char *WimaController::altitudeName = "Altitude";
const char *WimaController::snakeTileWidthName = "SnakeTileWidth";
const char *WimaController::snakeTileHeightName = "SnakeTileHeight";
const char *WimaController::snakeMinTileAreaName = "SnakeMinTileArea";
const char *WimaController::snakeLineDistanceName = "SnakeLineDistance";
const char *WimaController::snakeMinTransectLengthName =
"SnakeMinTransectLength";
WimaController::StatusMap WimaController::_nemoStatusMap{
std::make_pair<int, QString>(0, "No Heartbeat"),
......@@ -71,14 +71,7 @@ WimaController::WimaController(QObject *parent)
_flightSpeed(settingsGroup, _metaDataMap[flightSpeedName]),
_arrivalReturnSpeed(settingsGroup, _metaDataMap[arrivalReturnSpeedName]),
_altitude(settingsGroup, _metaDataMap[altitudeName]),
_snakeTileWidth(settingsGroup, _metaDataMap[snakeTileWidthName]),
_snakeTileHeight(settingsGroup, _metaDataMap[snakeTileHeightName]),
_snakeMinTileArea(settingsGroup, _metaDataMap[snakeMinTileAreaName]),
_snakeLineDistance(settingsGroup, _metaDataMap[snakeLineDistanceName]),
_snakeMinTransectLength(settingsGroup,
_metaDataMap[snakeMinTransectLengthName]),
_lowBatteryHandlingTriggered(false), _measurementPathLength(-1),
_snakeThread(this), _emptyThread(this), _currentThread(&_emptyThread),
_nemoInterface(this),
_batteryLevelTicker(EVENT_TIMER_INTERVAL, 1000 /*ms*/) {
......@@ -98,6 +91,14 @@ WimaController::WimaController(QObject *parent)
connect(&_altitude, &Fact::rawValueChanged, this,
&WimaController::_updateAltitude);
// Nemo stuff.
connect(&_nemoInterface, &NemoInterface::progressChanged, this,
&WimaController::_progressChangedHandler);
connect(&_nemoInterface, &NemoInterface::statusChanged, this,
&WimaController::nemoStatusChanged);
connect(&_nemoInterface, &NemoInterface::statusChanged, this,
&WimaController::nemoStatusStringChanged);
// Init waypoint managers.
bool value;
size_t overlap = _overlapWaypoints.rawValue().toUInt(&value);
......@@ -118,14 +119,6 @@ WimaController::WimaController(QObject *parent)
&WimaController::_eventTimerHandler);
_eventTimer.start(EVENT_TIMER_INTERVAL);
// SnakeThread.
connect(_currentThread, &SnakeThread::finished, this,
&WimaController::_threadFinishedHandler);
connect(_currentThread, &SnakeThread::calcInProgressChanged, this,
&WimaController::snakeCalcInProgressChanged);
connect(this, &QObject::destroyed, &this->_snakeThread, &SnakeThread::quit);
connect(this, &QObject::destroyed, &this->_emptyThread, &SnakeThread::quit);
// NemoInterface.
connect(&_nemoInterface, &NemoInterface::progressChanged, this,
&WimaController::_progressChangedHandler);
......@@ -199,16 +192,15 @@ Fact *WimaController::arrivalReturnSpeed() { return &_arrivalReturnSpeed; }
Fact *WimaController::altitude() { return &_altitude; }
QmlObjectListModel *WimaController::snakeTiles() {
static QmlObjectListModel list;
return &list;
return &this->_measurementArea.tiles();
}
QVariantList WimaController::snakeTileCenterPoints() {
return _currentThread->tileCenterPoints();
return this->_measurementArea.tileCenterPoints();
}
QVector<int> WimaController::nemoProgress() {
return _currentThread->progress();
return this->_measurementArea.progress();
}
double WimaController::phaseDistance() const {
......@@ -230,7 +222,8 @@ QString WimaController::nemoStatusString() const {
}
bool WimaController::snakeCalcInProgress() const {
return _currentThread->calcInProgress();
qWarning() << "using snakeCalcInProgress dummy";
return true;
}
void WimaController::setMasterController(PlanMasterController *masterC) {
......@@ -387,12 +380,18 @@ bool WimaController::setWimaPlanData(QSharedPointer<WimaPlanData> planData) {
_areas.clear();
_defaultWM.clear();
_snakeWM.clear();
_measurementArea = WimaMeasurementAreaData();
_serviceArea = WimaServiceAreaData();
_corridor = WimaCorridorData();
_joinedArea = WimaJoinedAreaData();
emit visualItemsChanged();
emit missionItemsChanged();
emit currentMissionItemsChanged();
emit waypointPathChanged();
emit currentWaypointPathChanged();
emit snakeTilesChanged();
emit nemoProgressChanged();
_localPlanDataValid = false;
......@@ -451,8 +450,26 @@ bool WimaController::setWimaPlanData(QSharedPointer<WimaPlanData> planData) {
}
emit visualItemsChanged();
emit snakeTilesChanged();
// Publish tiles if necessary.
if (!this->_nemoInterface.hasTileData(this->_measurementArea.tileData())) {
this->_nemoInterface.publishTileData(this->_measurementArea.tileData());
this->_measurementArea.progress() =
QVector<int>(this->_measurementArea.tiles()->count, 0);
emit nemoProgressChanged();
} else if (this->_enableSnake.rawValue().toBool()) {
const auto progess = this->_nemoInterface.progress();
if (progess.size() == this->_measurementArea.tiles()->count()) {
this->_measurementArea.progress() = std::move(progress);
emit nemoProgressChanged();
}
}
// Copy transects.
// Copy raw transects.
this->_rawTransects = planData->transects();
// Copy missionItems.
auto tempMissionItems = planData->missionItems();
if (tempMissionItems.size() < 1) {
qWarning("WimaController: Mission items from WimaPlaner empty!");
......@@ -475,12 +492,8 @@ bool WimaController::setWimaPlanData(QSharedPointer<WimaPlanData> planData) {
emit currentMissionItemsChanged();
emit waypointPathChanged();
emit currentWaypointPathChanged();
// qWarning("WimaController: Mission items from WimaPlaner empty!");
// return false;
_snakeThread.setMeasurementArea(_measurementArea.coordinateList());
_snakeThread.setServiceArea(_serviceArea.coordinateList());
_snakeThread.setCorridor(_corridor.coordinateList());
_currentThread->start();
qWarning("WimaController: add snake update here lkjdfl!");
_localPlanDataValid = true;
return true;
......@@ -760,50 +773,12 @@ void WimaController::_initSmartRTL() {
}
void WimaController::_threadFinishedHandler() {
#ifdef SNAKE_SHOW_TIME
qWarning() << "update here as well .jisddf";
auto start = std::chrono::high_resolution_clock::now();
#endif
if (!_snakeThread.errorMessage().isEmpty()) {
qDebug() << _snakeThread.errorMessage();
}
if (_snakeThread.tilesUpdated()) {
this->tiles.clearAndDeleteContents();
auto tls = _snakeThread.tiles().polygons();
for (const auto &t : tls) {
this->tiles.append(new SnakeTile(t, &tiles));
}
emit snakeTilesChanged();
emit snakeTileCenterPointsChanged();
_nemoInterface.setTilesENU(_snakeThread.tilesENU());
_nemoInterface.setENUOrigin(_snakeThread.ENUOrigin());
}
#ifdef SNAKE_SHOW_TIME
auto end = std::chrono::high_resolution_clock::now();
std::cout << "WimaController::_threadFinishedHandler(): tiles: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(end -
start)
.count()
<< " ms" << std::endl;
start = std::chrono::high_resolution_clock::now();
#endif
if (_snakeThread.progressUpdated()) {
emit nemoProgressChanged();
}
#ifdef SNAKE_SHOW_TIME
end = std::chrono::high_resolution_clock::now();
std::cout << "WimaController::_threadFinishedHandler(): progress: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(end -
start)
.count()
<< " ms" << std::endl;
start = std::chrono::high_resolution_clock::now();
#endif
if (_snakeThread.waypointsUpdated()) {
// Copy waypoints to waypoint manager.
_snakeWM.clear();
auto waypoints = _snakeThread.waypoints();
auto waypoints = 1;
if (waypoints.size() < 1) {
return;
}
......@@ -818,9 +793,8 @@ void WimaController::_threadFinishedHandler() {
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
emit waypointPathChanged();
}
#ifdef SNAKE_SHOW_TIME
end = std::chrono::high_resolution_clock::now();
auto end = std::chrono::high_resolution_clock::now();
std::cout << "WimaController::_threadFinishedHandler(): waypoints: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(end -
start)
......@@ -837,60 +811,56 @@ void WimaController::_switchToSnakeWaypointManager(QVariant variant) {
}
}
void WimaController::_switchThreadObject(SnakeThread &thread) {
if (_currentThread != &thread) {
// SnakeThread.
disconnect(_currentThread, &SnakeThread::finished, this,
&WimaController::_threadFinishedHandler);
disconnect(_currentThread, &SnakeThread::calcInProgressChanged, this,
&WimaController::snakeCalcInProgressChanged);
// NemoInterface.
disconnect(&_nemoInterface, &NemoInterface::progressChanged, this,
&WimaController::_progressChangedHandler);
disconnect(&_nemoInterface, &NemoInterface::statusChanged, this,
&WimaController::nemoStatusChanged);
disconnect(&_nemoInterface, &NemoInterface::statusChanged, this,
&WimaController::nemoStatusStringChanged);
_currentThread = &thread;
// SnakeThread.
connect(_currentThread, &SnakeThread::finished, this,
&WimaController::_threadFinishedHandler);
connect(_currentThread, &SnakeThread::calcInProgressChanged, this,
&WimaController::snakeCalcInProgressChanged);
// NemoInterface.
connect(&_nemoInterface, &NemoInterface::progressChanged, this,
&WimaController::_progressChangedHandler);
connect(&_nemoInterface, &NemoInterface::statusChanged, this,
&WimaController::nemoStatusChanged);
connect(&_nemoInterface, &NemoInterface::statusChanged, this,
&WimaController::nemoStatusStringChanged);
emit snakeCalcInProgressChanged();
emit snakeTilesChanged();
emit snakeTileCenterPointsChanged();
void WimaController::_progressChangedHandler() {
const auto progress = this->_nemoInterface.progress();
if (this->_measurementArea.tiles().count() == progress.size()) {
this->_measurementArea.progress() = std::move(progress);
emit nemoProgressChanged();
emit nemoStatusChanged();
emit nemoStatusStringChanged();
} else if (_localPlanDataValid) {
this->_nemoInterface.publishTileData(this->_measurementArea.tileData());
}
}
void WimaController::_progressChangedHandler() {
this->_snakeThread.setProgress(this->_nemoInterface.progress());
this->_snakeThread.start();
}
void WimaController::_enableSnakeChangedHandler() {
if (this->_enableSnake.rawValue().toBool()) {
qDebug() << "WimaController: enabling snake.";
_switchThreadObject(this->_snakeThread);
this->_nemoInterface.start();
this->_snakeThread.start();
} else {
qDebug() << "WimaController: disabling snake.";
this->_nemoInterface.stop();
this->_snakeThread.quit();
_switchThreadObject(_emptyThread);
}
}
void WimaController::_updateRoute() {
if (_localPlanDataValid && this->_joinedArea.coordinateList().size() > 0) {
auto safeArea = this->_joinedArea.coordinateList();
for (auto &v : safeArea) {
v.setAltitude(0);
}
const auto &origin = safeArea.front();
snake::BoostPolygon safeAreaENU;
snake::areaToEnu(origin, safeArea, safeAreaENU);
const auto &depot = this->_serviceArea.depot();
const auto &geoTransects = this->_rawTransects;
const auto progess = this->_nemoInterface.progress();
auto pTiles = std::make_shared<QList<QList<QGeoCoordinate>>>();
auto generator = [origin, depot,
geoTransects](snake::Transects &transects) -> bool {
snake::BoostPolygon depotENU;
snake::toENU(origin, depot, depotENU);
transects.push_back(snake::BoostLineString{depotENU});
for (auto &geoTransect : geoTransects) {
snake::BoostLineString t;
for (auto &geoVertex : geoTransect) {
snake::BoostPoint v;
snake::toENU(origin, geoVertex, v);
t.push_back(v);
}
transects.push_back(t);
}
return true;
};
this->_routingThread.route(safeAreaENU, generator);
}
}
src/Wima/WimaController.h
View file @ f060c3b7
......@@ -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;
......
Supports Markdown
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