Commit 5e62e791 authored by Valentin Platzgummer's avatar Valentin Platzgummer

wima planer state machine added

parent dedb0c51
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
...@@ -454,6 +454,7 @@ HEADERS += \ ...@@ -454,6 +454,7 @@ HEADERS += \
src/Wima/Snake/SnakeTileLocal.h \ src/Wima/Snake/SnakeTileLocal.h \
src/Wima/Snake/SnakeTiles.h \ src/Wima/Snake/SnakeTiles.h \
src/Wima/Snake/SnakeTilesLocal.h \ src/Wima/Snake/SnakeTilesLocal.h \
src/Wima/StateMachine.h \
src/Wima/WaypointManager/AreaInterface.h \ src/Wima/WaypointManager/AreaInterface.h \
src/Wima/WaypointManager/DefaultManager.h \ src/Wima/WaypointManager/DefaultManager.h \
src/Wima/WaypointManager/GenericWaypointManager.h \ src/Wima/WaypointManager/GenericWaypointManager.h \
...@@ -515,6 +516,7 @@ SOURCES += \ ...@@ -515,6 +516,7 @@ SOURCES += \
src/Wima/Snake/NemoInterface.cpp \ src/Wima/Snake/NemoInterface.cpp \
src/Wima/Snake/QNemoProgress.cc \ src/Wima/Snake/QNemoProgress.cc \
src/Wima/Snake/SnakeTile.cpp \ src/Wima/Snake/SnakeTile.cpp \
src/Wima/StateMachine.cpp \
src/Wima/WaypointManager/AreaInterface.cpp \ src/Wima/WaypointManager/AreaInterface.cpp \
src/Wima/WaypointManager/DefaultManager.cpp \ src/Wima/WaypointManager/DefaultManager.cpp \
src/Wima/WaypointManager/GenericWaypointManager.cpp \ src/Wima/WaypointManager/GenericWaypointManager.cpp \
......
This diff is collapsed.
...@@ -6,6 +6,9 @@ ...@@ -6,6 +6,9 @@
#include "SettingsFact.h" #include "SettingsFact.h"
#include "TransectStyleComplexItem.h" #include "TransectStyleComplexItem.h"
#include "Geometry/WimaJoinedAreaData.h"
#include "Geometry/WimaMeasurementAreaData.h"
class RoutingThread; class RoutingThread;
class RoutingData; class RoutingData;
...@@ -50,8 +53,10 @@ public: ...@@ -50,8 +53,10 @@ public:
// Property setters // Property setters
void setRefPoint(const QGeoCoordinate &refPt); void setRefPoint(const QGeoCoordinate &refPt);
void setHidePolygon(bool hide); void setHidePolygon(bool hide);
void setDepot(const QGeoCoordinate &depot); void setMeasurementArea(const WimaMeasurementAreaData &mArea);
void setSafeArea(const QList<QGeoCoordinate> &safeArea); void setJoinedArea(const WimaJoinedAreaData &jArea);
void setMeasurementArea(const WimaMeasurementArea &mArea);
void setJoinedArea(const WimaJoinedArea &jArea);
// Property getters // Property getters
QGeoCoordinate refPoint() const; QGeoCoordinate refPoint() const;
...@@ -68,7 +73,6 @@ public: ...@@ -68,7 +73,6 @@ public:
QList<QString> variantNames() const; QList<QString> variantNames() const;
QList<QString> runNames() const; QList<QString> runNames() const;
QGeoCoordinate depot() const; QGeoCoordinate depot() const;
QList<QGeoCoordinate> safeArea() const;
const QList<QList<QGeoCoordinate>> &rawTransects() const; const QList<QList<QGeoCoordinate>> &rawTransects() const;
// Overrides // Overrides
...@@ -105,9 +109,10 @@ signals: ...@@ -105,9 +109,10 @@ signals:
void calculatingChanged(); void calculatingChanged();
void hidePolygonChanged(); void hidePolygonChanged();
void depotChanged(); void depotChanged();
void safeAreaChanged();
void variantNamesChanged(); void variantNamesChanged();
void runNamesChanged(); void runNamesChanged();
void measurementAreaChanged();
void joinedAreaChanged();
private slots: private slots:
// Overrides from TransectStyleComplexItem // Overrides from TransectStyleComplexItem
...@@ -147,14 +152,16 @@ private: ...@@ -147,14 +152,16 @@ private:
SettingsFact _run; SettingsFact _run;
QList<QString> _runNames; QList<QString> _runNames;
// Area data
WimaMeasurementAreaData _mArea;
WimaJoinedAreaData _jArea;
// Worker // Worker
using PtrWorker = std::shared_ptr<RoutingThread>; using PtrWorker = std::shared_ptr<RoutingThread>;
PtrWorker _pWorker; PtrWorker _pWorker;
PtrRoutingData _pRoutingData; PtrRoutingData _pRoutingData;
// Routing data. // Routing data.
QGeoCoordinate _depot;
QList<QGeoCoordinate> _safeArea;
QList<QList<QGeoCoordinate>> _rawTransects; QList<QList<QGeoCoordinate>> _rawTransects;
using Runs = QVector<Transects>; using Runs = QVector<Transects>;
QVector<Runs> _variantVector; QVector<Runs> _variantVector;
......
...@@ -8,7 +8,7 @@ ...@@ -8,7 +8,7 @@
#include "boost/units/systems/angle/degrees.hpp" #include "boost/units/systems/angle/degrees.hpp"
#include "boost/units/systems/si.hpp" #include "boost/units/systems/si.hpp"
template <class Point, int k> auto get(Point &p); template <int k, class Point> auto get(const Point &p);
namespace bu = boost::units; namespace bu = boost::units;
...@@ -112,11 +112,11 @@ void approximate(Circle &circ, long n, qty::Angle alpha1, qty::Angle alpha2, ...@@ -112,11 +112,11 @@ void approximate(Circle &circ, long n, qty::Angle alpha1, qty::Angle alpha2,
} }
double a = a1; double a = a1;
using Point =
std::remove_cv_t<std::remove_reference_t<decltype(circ.origin())>>;
auto x0 = get<0>(circ.origin()); auto x0 = get<0>(circ.origin());
auto y0 = get<1>(circ.origin()); auto y0 = get<1>(circ.origin());
auto r = circ.radius(); auto r = circ.radius();
using Point =
std::remove_cv_t<std::remove_reference_t<decltype(circ.origin())>>;
using NumberType = std::remove_cv_t<std::remove_reference_t<decltype(x0)>>; using NumberType = std::remove_cv_t<std::remove_reference_t<decltype(x0)>>;
while (n--) { while (n--) {
auto x = NumberType(x0 + r * std::cos(a)); auto x = NumberType(x0 + r * std::cos(a));
...@@ -139,11 +139,11 @@ void approximate(Circle &circ, long n, Container &c) { ...@@ -139,11 +139,11 @@ void approximate(Circle &circ, long n, Container &c) {
} }
double a = 0; double a = 0;
using Point =
std::remove_cv_t<std::remove_reference_t<decltype(circ.origin())>>;
auto x0 = get<0>(circ.origin()); auto x0 = get<0>(circ.origin());
auto y0 = get<1>(circ.origin()); auto y0 = get<1>(circ.origin());
auto r = circ.radius(); auto r = circ.radius();
using Point =
std::remove_cv_t<std::remove_reference_t<decltype(circ.origin())>>;
using NumberType = std::remove_cv_t<std::remove_reference_t<decltype(x0)>>; using NumberType = std::remove_cv_t<std::remove_reference_t<decltype(x0)>>;
while (n--) { while (n--) {
auto x = NumberType(x0 + r * std::cos(a)); auto x = NumberType(x0 + r * std::cos(a));
......
#include "WimaAreaData.h" #include "WimaAreaData.h"
WimaAreaData::WimaAreaData(QObject *parent) : QObject(parent) { WimaAreaData::WimaAreaData(QObject *parent) : QObject(parent) {}
_maxAltitude = 0;
}
WimaAreaData::~WimaAreaData() {} WimaAreaData::~WimaAreaData() {}
/*! bool WimaAreaData::operator==(const WimaAreaData &data) const {
* \fn double WimaAreaData::maxAltitude() return this->_path == data._path && this->_center == data._center;
* }
* Returns the maximum altitude at which vehicles are allowed to fly.
*/ bool WimaAreaData::operator!=(const WimaAreaData &data) const {
double WimaAreaData::maxAltitude() const { return _maxAltitude; } return !this->operator==(data);
}
/*!
* \fn double WimaAreaData::maxAltitude()
*
* Returns the path (vertex list defining the \l {Simple Polygon}).
*/
QVariantList WimaAreaData::path() const { return _path; } QVariantList WimaAreaData::path() const { return _path; }
QGeoCoordinate WimaAreaData::center() const { return _center; } QGeoCoordinate WimaAreaData::center() const { return _center; }
...@@ -51,20 +45,6 @@ void WimaAreaData::clear() { ...@@ -51,20 +45,6 @@ void WimaAreaData::clear() {
_path.clear(); _path.clear();
} }
/*!
* \fn void WimaAreaData::setMaxAltitude(double maxAltitude)
*
* Sets the maximum altitude member to \a maxAltitude and emits the \c
* maxAltitudeChanged() signal if \a maxAltitude differs from the members value.
*/
void WimaAreaData::setMaxAltitude(double maxAltitude) {
if (!qFuzzyCompare(_maxAltitude, maxAltitude)) {
_maxAltitude = maxAltitude;
emit maxAltitudeChanged(_maxAltitude);
}
}
void WimaAreaData::setPath(const QVariantList &coordinateList) { void WimaAreaData::setPath(const QVariantList &coordinateList) {
_path = coordinateList; _path = coordinateList;
_list.clear(); _list.clear();
...@@ -87,13 +67,11 @@ void WimaAreaData::setCenter(const QGeoCoordinate &center) { ...@@ -87,13 +67,11 @@ void WimaAreaData::setCenter(const QGeoCoordinate &center) {
* Assigns \a other to the invoking object * Assigns \a other to the invoking object
*/ */
void WimaAreaData::assign(const WimaAreaData &other) { void WimaAreaData::assign(const WimaAreaData &other) {
setMaxAltitude(other.maxAltitude());
setPath(other.path()); setPath(other.path());
setCenter(other.center()); setCenter(other.center());
} }
void WimaAreaData::assign(const WimaArea &other) { void WimaAreaData::assign(const WimaArea &other) {
setMaxAltitude(other.maxAltitude());
setPath(other.path()); setPath(other.path());
setCenter(other.center()); setCenter(other.center());
} }
...@@ -116,6 +94,13 @@ void WimaAreaData::setPath(const QList<QGeoCoordinate> &coordinateList) { ...@@ -116,6 +94,13 @@ void WimaAreaData::setPath(const QList<QGeoCoordinate> &coordinateList) {
emit pathChanged(_path); emit pathChanged(_path);
} }
bool operator==(const WimaAreaData &m1, const WimaArea &m2) {
return m1.path() == m2.path() && m1.center() == m2.center();
}
bool operator!=(const WimaAreaData &m1, const WimaArea &m2) {
return !operator==(m1, m2);
}
/*! /*!
* \class WimaArea::WimaAreaData * \class WimaArea::WimaAreaData
* \brief Class to store and exchange data of a \c WimaArea Object. * \brief Class to store and exchange data of a \c WimaArea Object.
......
...@@ -20,7 +20,9 @@ public: ...@@ -20,7 +20,9 @@ public:
WimaAreaData & WimaAreaData &
operator=(const WimaAreaData &otherData) = delete; // avoid slicing operator=(const WimaAreaData &otherData) = delete; // avoid slicing
double maxAltitude() const; bool operator==(const WimaAreaData &data) const;
bool operator!=(const WimaAreaData &data) const;
QVariantList path() const; QVariantList path() const;
QGeoCoordinate center() const; QGeoCoordinate center() const;
const QList<QGeoCoordinate> &coordinateList() const; const QList<QGeoCoordinate> &coordinateList() const;
...@@ -32,12 +34,10 @@ public: ...@@ -32,12 +34,10 @@ public:
void clear(); void clear();
signals: signals:
void maxAltitudeChanged(double maxAltitude);
void pathChanged(const QVariantList &coordinateList); void pathChanged(const QVariantList &coordinateList);
void centerChanged(void); void centerChanged(void);
public slots: public slots:
void setMaxAltitude(double maxAltitude);
void setPath(const QList<QGeoCoordinate> &coordinateList); void setPath(const QList<QGeoCoordinate> &coordinateList);
void setPath(const QVariantList &coordinateList); void setPath(const QVariantList &coordinateList);
void setCenter(const QGeoCoordinate &center); void setCenter(const QGeoCoordinate &center);
...@@ -51,8 +51,10 @@ private: ...@@ -51,8 +51,10 @@ private:
// Member Variables // Member Variables
// see WimaArea.h for explanation // see WimaArea.h for explanation
double _maxAltitude;
QVariantList _path; QVariantList _path;
QList<QGeoCoordinate> _list; mutable QList<QGeoCoordinate> _list;
QGeoCoordinate _center; QGeoCoordinate _center;
}; };
bool operator==(const WimaAreaData &m1, const WimaArea &m2);
bool operator!=(const WimaAreaData &m1, const WimaArea &m2);
...@@ -5,10 +5,14 @@ ...@@ -5,10 +5,14 @@
#include <boost/units/systems/si.hpp> #include <boost/units/systems/si.hpp>
#include "QGCLoggingCategory.h"
#ifndef SNAKE_MAX_TILES #ifndef SNAKE_MAX_TILES
#define SNAKE_MAX_TILES 1000 #define SNAKE_MAX_TILES 1000
#endif #endif
QGC_LOGGING_CATEGORY(WimaMeasurementAreaLog, "WimaMeasurementAreaLog")
TileData::TileData() : tiles(this) {} TileData::TileData() : tiles(this) {}
TileData::~TileData() { tiles.clearAndDeleteContents(); } TileData::~TileData() { tiles.clearAndDeleteContents(); }
...@@ -21,7 +25,7 @@ TileData &TileData::operator=(const TileData &other) { ...@@ -21,7 +25,7 @@ TileData &TileData::operator=(const TileData &other) {
if (tile != nullptr) { if (tile != nullptr) {
this->tiles.append(new SnakeTile(*tile, this)); this->tiles.append(new SnakeTile(*tile, this));
} else { } else {
qWarning("TileData::operator=: nullptr"); qCWarning(WimaMeasurementAreaLog) << "TileData::operator=: nullptr";
} }
} }
this->tileCenterPoints = other.tileCenterPoints; this->tileCenterPoints = other.tileCenterPoints;
...@@ -80,7 +84,7 @@ WimaMeasurementArea::WimaMeasurementArea(QObject *parent) ...@@ -80,7 +84,7 @@ WimaMeasurementArea::WimaMeasurementArea(QObject *parent)
this /* QObject parent */)), this /* QObject parent */)),
_showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName], _showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName],
this /* QObject parent */)), this /* QObject parent */)),
_calculating(false) { _state(STATE::IDLE) {
init(); init();
} }
...@@ -104,7 +108,7 @@ WimaMeasurementArea::WimaMeasurementArea(const WimaMeasurementArea &other, ...@@ -104,7 +108,7 @@ WimaMeasurementArea::WimaMeasurementArea(const WimaMeasurementArea &other,
this /* QObject parent */)), this /* QObject parent */)),
_showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName], _showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesName],
this /* QObject parent */)), this /* QObject parent */)),
_calculating(false) { _state(STATE::IDLE) {
init(); init();
} }
...@@ -123,11 +127,11 @@ operator=(const WimaMeasurementArea &other) { ...@@ -123,11 +127,11 @@ operator=(const WimaMeasurementArea &other) {
WimaMeasurementArea::~WimaMeasurementArea() {} WimaMeasurementArea::~WimaMeasurementArea() {}
QString WimaMeasurementArea::mapVisualQML() const { QString WimaMeasurementArea::mapVisualQML() const {
return "WimaMeasurementAreaMapVisual.qml"; return QStringLiteral("WimaMeasurementAreaMapVisual.qml");
} }
QString WimaMeasurementArea::editorQML() const { QString WimaMeasurementArea::editorQML() const {
return "WimaMeasurementAreaEditor.qml"; return QStringLiteral("WimaMeasurementAreaEditor.qml");
} }
Fact *WimaMeasurementArea::tileHeight() { return &_tileHeight; } Fact *WimaMeasurementArea::tileHeight() { return &_tileHeight; }
...@@ -146,7 +150,13 @@ QmlObjectListModel *WimaMeasurementArea::tiles() { ...@@ -146,7 +150,13 @@ QmlObjectListModel *WimaMeasurementArea::tiles() {
return &this->_tileData.tiles; return &this->_tileData.tiles;
} }
QVector<int> WimaMeasurementArea::progress() { return this->_progress; } const QVector<int> &WimaMeasurementArea::progress() const {
return this->_progress;
}
QVector<int> WimaMeasurementArea::progressQml() const {
return this->_progress;
}
const QmlObjectListModel *WimaMeasurementArea::tiles() const { const QmlObjectListModel *WimaMeasurementArea::tiles() const {
return &this->_tileData.tiles; return &this->_tileData.tiles;
...@@ -162,7 +172,7 @@ const TileData &WimaMeasurementArea::tileData() const { ...@@ -162,7 +172,7 @@ const TileData &WimaMeasurementArea::tileData() const {
int WimaMeasurementArea::maxTiles() const { return SNAKE_MAX_TILES; } int WimaMeasurementArea::maxTiles() const { return SNAKE_MAX_TILES; }
bool WimaMeasurementArea::ready() const { return !_calculating; } bool WimaMeasurementArea::ready() const { return this->_state == STATE::IDLE; }
void WimaMeasurementArea::saveToJson(QJsonObject &json) { void WimaMeasurementArea::saveToJson(QJsonObject &json) {
this->WimaArea::saveToJson(json); this->WimaArea::saveToJson(json);
...@@ -230,7 +240,7 @@ bool WimaMeasurementArea::loadFromJson(const QJsonObject &json, ...@@ -230,7 +240,7 @@ bool WimaMeasurementArea::loadFromJson(const QJsonObject &json,
} }
bool WimaMeasurementArea::setProgress(const QVector<int> &p) { bool WimaMeasurementArea::setProgress(const QVector<int> &p) {
if (!_calculating) { if (!ready()) {
if (p.size() == this->tiles()->count() && this->_progress != p) { if (p.size() == this->tiles()->count() && this->_progress != p) {
this->_progress = p; this->_progress = p;
emit progressChanged(); emit progressChanged();
...@@ -246,18 +256,18 @@ bool WimaMeasurementArea::setProgress(const QVector<int> &p) { ...@@ -246,18 +256,18 @@ bool WimaMeasurementArea::setProgress(const QVector<int> &p) {
void WimaMeasurementArea::doUpdate() { void WimaMeasurementArea::doUpdate() {
using namespace snake; using namespace snake;
using namespace boost::units; using namespace boost::units;
#ifdef SNAKE_SHOW_TIME
auto start = std::chrono::high_resolution_clock::now(); auto start = std::chrono::high_resolution_clock::now();
#endif
const auto height = this->_tileHeight.rawValue().toDouble() * si::meter; const auto height = this->_tileHeight.rawValue().toDouble() * si::meter;
const auto width = this->_tileWidth.rawValue().toDouble() * si::meter; const auto width = this->_tileWidth.rawValue().toDouble() * si::meter;
const auto tileArea = width * height; const auto tileArea = width * height;
const auto totalArea = this->area() * si::meter * si::meter; const auto totalArea = this->area() * si::meter * si::meter;
const auto estNumTiles = totalArea / tileArea; const auto estNumTiles = totalArea / tileArea;
if (!this->_calculating && if (this->_state != STATE::UPDATE &&
long(std::ceil(estNumTiles.value())) <= SNAKE_MAX_TILES && long(std::ceil(estNumTiles.value())) <= SNAKE_MAX_TILES &&
this->count() >= 3 && this->isSimplePolygon()) { this->count() >= 3 && this->isSimplePolygon()) {
this->_calculating = true; setState(STATE::UPDATE);
auto polygon = this->coordinateList(); auto polygon = this->coordinateList();
for (auto &v : polygon) { for (auto &v : polygon) {
v.setAltitude(0); v.setAltitude(0);
...@@ -266,9 +276,8 @@ void WimaMeasurementArea::doUpdate() { ...@@ -266,9 +276,8 @@ void WimaMeasurementArea::doUpdate() {
this->_minTileArea.rawValue().toDouble() * si::meter * si::meter; this->_minTileArea.rawValue().toDouble() * si::meter * si::meter;
auto *th = this->thread(); auto *th = this->thread();
auto future = QtConcurrent::run([polygon, th, height, width, minArea] { auto future = QtConcurrent::run([polygon, th, height, width, minArea] {
#ifdef SNAKE_SHOW_TIME
auto start = std::chrono::high_resolution_clock::now(); auto start = std::chrono::high_resolution_clock::now();
#endif
DataPtr pData(new TileData()); DataPtr pData(new TileData());
// Convert to ENU system. // Convert to ENU system.
QGeoCoordinate origin = polygon.first(); QGeoCoordinate origin = polygon.first();
...@@ -298,56 +307,67 @@ void WimaMeasurementArea::doUpdate() { ...@@ -298,56 +307,67 @@ void WimaMeasurementArea::doUpdate() {
} }
} }
pData->moveToThread(th); pData->moveToThread(th);
#ifdef SNAKE_SHOW_TIME
qDebug() << "WimaMeasurementArea::doUpdate concurrent update execution " qCDebug(WimaMeasurementAreaLog)
"time: " << "doUpdate(): update time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>( << std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start) std::chrono::high_resolution_clock::now() - start)
.count() .count()
<< " ms"; << " ms";
#endif
return pData; return pData;
}); // QtConcurrent::run() }); // QtConcurrent::run()
this->_watcher.setFuture(future); this->_watcher.setFuture(future);
} }
#ifdef SNAKE_SHOW_TIME
qDebug() << "WimaMeasurementArea::doUpdate execution time: " qCDebug(WimaMeasurementAreaLog)
<< std::chrono::duration_cast<std::chrono::milliseconds>( << "doUpdate(): execution time: "
std::chrono::high_resolution_clock::now() - start) << std::chrono::duration_cast<std::chrono::milliseconds>(
.count() std::chrono::high_resolution_clock::now() - start)
<< " ms"; .count()
#endif << " ms";
} }
void WimaMeasurementArea::deferUpdate() { void WimaMeasurementArea::deferUpdate() {
if (this->_timer.isActive()) { if (this->_state == STATE::IDLE || this->_state == STATE::DEFERED) {
this->_timer.stop(); if (this->_state == STATE::IDLE) {
} this->_progress.clear();
if (this->_tileData.size() > 0) { this->_tileData.clear();
this->_progress.clear(); emit this->progressChanged();
emit this->progressChanged(); emit this->tilesChanged();
this->_tileData.clear(); }
emit this->tilesChanged(); this->setState(STATE::DEFERED);
this->_timer.start(100);
} else if (this->_state == STATE::UPDATE) {
setState(STATE::RESTART);
} }
this->_timer.start(100);
} }
void WimaMeasurementArea::storeTiles() { void WimaMeasurementArea::storeTiles() {
#ifdef SNAKE_SHOW_TIME
auto start = std::chrono::high_resolution_clock::now(); auto start = std::chrono::high_resolution_clock::now();
#endif
this->_tileData = *this->_watcher.result(); if (this->_state == STATE::UPDATE) {
this->_calculating = false;
// This is expensive. Drawing tiles is expensive too. qCDebug(WimaMeasurementAreaLog) << "storeTiles(): update.";
emit this->tilesChanged();
#ifdef SNAKE_SHOW_TIME this->_tileData = *this->_watcher.result();
qDebug() << "WimaMeasurementArea::storeTiles() execution time: " // This is expensive. Drawing tiles is expensive too.
<< std::chrono::duration_cast<std::chrono::milliseconds>( this->_progress = QVector<int>(this->_tileData.tiles.count(), 0);
std::chrono::high_resolution_clock::now() - start) this->progressChanged();
.count() emit this->tilesChanged();
<< " ms"; setState(STATE::IDLE);
#endif } else if (this->_state == STATE::RESTART) {
qCDebug(WimaMeasurementAreaLog) << "storeTiles(): restart.";
doUpdate();
}
qCDebug(WimaMeasurementAreaLog)
<< "storeTiles() execution time: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start)
.count()
<< " ms";
} }
void WimaMeasurementArea::init() { void WimaMeasurementArea::init() {
...@@ -368,6 +388,16 @@ void WimaMeasurementArea::init() { ...@@ -368,6 +388,16 @@ void WimaMeasurementArea::init() {
&WimaMeasurementArea::storeTiles); &WimaMeasurementArea::storeTiles);
} }
void WimaMeasurementArea::setState(WimaMeasurementArea::STATE s) {
if (this->_state != s) {
auto oldState = this->_state;
this->_state = s;
if (s == STATE::IDLE || oldState == STATE::IDLE) {
emit readyChanged();
}
}
}
/*! /*!
* \class WimaMeasurementArea * \class WimaMeasurementArea
* \brief Class defining the area inside which the actual drone measurements * \brief Class defining the area inside which the actual drone measurements
......
...@@ -26,6 +26,14 @@ public: ...@@ -26,6 +26,14 @@ public:
class WimaMeasurementArea : public WimaArea { class WimaMeasurementArea : public WimaArea {
Q_OBJECT Q_OBJECT
enum class STATE {
IDLE,
DEFERED,
UPDATE,
RESTART,
};
public: public:
WimaMeasurementArea(QObject *parent = nullptr); WimaMeasurementArea(QObject *parent = nullptr);
WimaMeasurementArea(const WimaMeasurementArea &other, WimaMeasurementArea(const WimaMeasurementArea &other,
...@@ -41,7 +49,7 @@ public: ...@@ -41,7 +49,7 @@ public:
Q_PROPERTY(Fact *showTiles READ showTiles CONSTANT) Q_PROPERTY(Fact *showTiles READ showTiles CONSTANT)
Q_PROPERTY(QmlObjectListModel *tiles READ tiles NOTIFY tilesChanged) Q_PROPERTY(QmlObjectListModel *tiles READ tiles NOTIFY tilesChanged)
Q_PROPERTY(int maxTiles READ maxTiles NOTIFY maxTilesChanged) Q_PROPERTY(int maxTiles READ maxTiles NOTIFY maxTilesChanged)
Q_PROPERTY(QVector<int> progress READ progress NOTIFY progressChanged) Q_PROPERTY(QVector<int> progress READ progressQml NOTIFY progressChanged)
// Overrides from WimaPolygon // Overrides from WimaPolygon
QString mapVisualQML(void) const; QString mapVisualQML(void) const;
...@@ -54,7 +62,8 @@ public: ...@@ -54,7 +62,8 @@ public:
Fact *minTransectLength(); Fact *minTransectLength();
Fact *showTiles(); Fact *showTiles();
QmlObjectListModel *tiles(); QmlObjectListModel *tiles();
QVector<int> progress(); const QVector<int> &progress() const;
QVector<int> progressQml() const;
const QmlObjectListModel *tiles() const; const QmlObjectListModel *tiles() const;
const QVariantList &tileCenterPoints() const; // List of QGeoCoordinate const QVariantList &tileCenterPoints() const; // List of QGeoCoordinate
const TileData &tileData() const; const TileData &tileData() const;
...@@ -83,6 +92,7 @@ signals: ...@@ -83,6 +92,7 @@ signals:
void tilesChanged(); void tilesChanged();
void maxTilesChanged(); void maxTilesChanged();
void progressChanged(); void progressChanged();
void readyChanged();
public slots: public slots:
bool setProgress(const QVector<int> &p); bool setProgress(const QVector<int> &p);
...@@ -95,6 +105,7 @@ private slots: ...@@ -95,6 +105,7 @@ private slots:
private: private:
// Member Methodes // Member Methodes
void init(); void init();
void setState(STATE s);
// Members // Members
QMap<QString, FactMetaData *> _metaDataMap; QMap<QString, FactMetaData *> _metaDataMap;
...@@ -111,7 +122,8 @@ private: ...@@ -111,7 +122,8 @@ private:
using DataPtr = std::shared_ptr<TileData>; using DataPtr = std::shared_ptr<TileData>;
TileData _tileData; TileData _tileData;
QFutureWatcher<DataPtr> _watcher; QFutureWatcher<DataPtr> _watcher;
bool _calculating;
STATE _state;
QVector<int> _progress; QVector<int> _progress;
}; };
...@@ -18,6 +18,18 @@ WimaMeasurementAreaData::WimaMeasurementAreaData( ...@@ -18,6 +18,18 @@ WimaMeasurementAreaData::WimaMeasurementAreaData(
*this = other; *this = other;
} }
bool WimaMeasurementAreaData::
operator==(const WimaMeasurementAreaData &other) const {
return this->WimaAreaData::operator==(other) &&
this->_tileData == other.tileData() &&
this->center() == other.center();
}
bool WimaMeasurementAreaData::
operator!=(const WimaMeasurementAreaData &other) const {
return !(*this == other);
}
/*! /*!
* \overload operator=(); * \overload operator=();
* *
...@@ -82,9 +94,31 @@ void WimaMeasurementAreaData::assign(const WimaMeasurementArea &other) { ...@@ -82,9 +94,31 @@ void WimaMeasurementAreaData::assign(const WimaMeasurementArea &other) {
WimaAreaData::assign(other); WimaAreaData::assign(other);
if (other.ready()) { if (other.ready()) {
this->_tileData = other.tileData(); this->_tileData = other.tileData();
qWarning() << "WimaMeasurementAreaData: add progress copy here."; this->_progress = other.progress();
} else { } else {
qWarning() qWarning()
<< "WimaMeasurementAreaData::assign(): WimaMeasurementArea not ready."; << "WimaMeasurementAreaData::assign(): WimaMeasurementArea not ready.";
} }
} }
bool operator==(const WimaMeasurementAreaData &m1,
const WimaMeasurementArea &m2) {
return operator==(*static_cast<const WimaAreaData *>(&m1),
*static_cast<const WimaArea *>(&m2)) &&
m1.tileData() == m2.tileData() && m1.progress() == m2.progress();
}
bool operator!=(const WimaMeasurementAreaData &m1,
const WimaMeasurementArea &m2) {
return !(m1 == m2);
}
bool operator==(const WimaMeasurementArea &m1,
const WimaMeasurementAreaData &m2) {
return m2 == m1;
}
bool operator!=(const WimaMeasurementArea &m1,
const WimaMeasurementAreaData &m2) {
return m2 != m1;
}
...@@ -18,6 +18,9 @@ public: ...@@ -18,6 +18,9 @@ public:
WimaMeasurementAreaData &operator=(const WimaMeasurementAreaData &other); WimaMeasurementAreaData &operator=(const WimaMeasurementAreaData &other);
WimaMeasurementAreaData &operator=(const WimaMeasurementArea &other); WimaMeasurementAreaData &operator=(const WimaMeasurementArea &other);
bool operator==(const WimaMeasurementAreaData &other) const;
bool operator!=(const WimaMeasurementAreaData &other) const;
QString type() const; QString type() const;
WimaMeasurementAreaData *Clone() const { WimaMeasurementAreaData *Clone() const {
return new WimaMeasurementAreaData(*this); return new WimaMeasurementAreaData(*this);
...@@ -42,3 +45,12 @@ private: ...@@ -42,3 +45,12 @@ private:
TileData _tileData; TileData _tileData;
QVector<int> _progress; QVector<int> _progress;
}; };
bool operator==(const WimaMeasurementAreaData &m1,
const WimaMeasurementArea &m2);
bool operator!=(const WimaMeasurementAreaData &m1,
const WimaMeasurementArea &m2);
bool operator==(const WimaMeasurementArea &m1,
const WimaMeasurementAreaData &m2);
bool operator!=(const WimaMeasurementArea &m1,
const WimaMeasurementAreaData &m2);
...@@ -4,6 +4,9 @@ ...@@ -4,6 +4,9 @@
// Qt // Qt
#include <QDebug> #include <QDebug>
#include "QGCLoggingCategory.h"
QGC_LOGGING_CATEGORY(RoutingWorkerLog, "RoutingWorkerLog")
RoutingThread::RoutingThread(QObject *parent) RoutingThread::RoutingThread(QObject *parent)
: QThread(parent), _calculating(false), _stop(false), _restart(false) { : QThread(parent), _calculating(false), _stop(false), _restart(false) {
...@@ -41,16 +44,13 @@ void RoutingThread::route(const RoutingParameter &par, ...@@ -41,16 +44,13 @@ void RoutingThread::route(const RoutingParameter &par,
} }
void RoutingThread::run() { void RoutingThread::run() {
qWarning() << "RoutingWorker::run(): thread start."; qCWarning(RoutingWorkerLog) << "run(): thread start.";
while (!this->_stop) { while (!this->_stop) {
#ifdef DEBUG_CIRCULAR_SURVEY qCWarning(RoutingWorkerLog) << "run(): calculation "
qWarning() << "RoutingWorker::run(): calculation " "started.";
"started.";
#endif
// Copy input. // Copy input.
#ifdef SHOW_CIRCULAR_SURVEY_TIME
auto start = std::chrono::high_resolution_clock::now(); auto start = std::chrono::high_resolution_clock::now();
#endif
this->_calculating = true; this->_calculating = true;
emit calculatingChanged(); emit calculatingChanged();
Lock lk(this->_mutex); Lock lk(this->_mutex);
...@@ -67,10 +67,8 @@ void RoutingThread::run() { ...@@ -67,10 +67,8 @@ void RoutingThread::run() {
if (generator(transectsENU)) { if (generator(transectsENU)) {
// Check if generation was successful. // Check if generation was successful.
if (transectsENU.size() == 0) { if (transectsENU.size() == 0) {
#ifdef DEBUG_CIRCULAR_SURVEY qCWarning(RoutingWorkerLog) << "run(): "
qWarning() << "RoutingWorker::run(): " "not able to generate transects.";
"not able to generate transects.";
#endif
} else { } else {
// Prepare data for routing. // Prepare data for routing.
auto &solutionVector = pRouteData->solutionVector; auto &solutionVector = pRouteData->solutionVector;
...@@ -95,38 +93,29 @@ void RoutingThread::run() { ...@@ -95,38 +93,29 @@ void RoutingThread::run() {
// Check if routing was successful. // Check if routing was successful.
if ((!success || solutionVector.size() < 1) && !this->_restart) { if ((!success || solutionVector.size() < 1) && !this->_restart) {
#ifdef DEBUG_CIRCULAR_SURVEY qCWarning(RoutingWorkerLog) << "run(): "
qWarning() << "RoutingWorker::run(): " "routing failed. "
"routing failed."; << snakePar.errorString.c_str();
qWarning() << snakePar.errorString.c_str();
#endif
} else if (this->_restart) { } else if (this->_restart) {
#ifdef DEBUG_CIRCULAR_SURVEY qCWarning(RoutingWorkerLog) << "run(): "
qWarning() << "RoutingWorker::run(): " "restart requested.";
"restart requested.";
#endif
} else { } else {
// Notify main thread. // Notify main thread.
emit result(pRouteData); emit result(pRouteData);
#ifdef DEBUG_CIRCULAR_SURVEY qCWarning(RoutingWorkerLog) << "run(): "
qWarning() << "RoutingWorker::run(): " "concurrent update success.";
"concurrent update success.";
#endif
} }
} }
} // end calculation } // end calculation
#ifdef DEBUG_CIRCULAR_SURVEY
else { else {
qWarning() << "RoutingWorker::run(): generator() failed."; qCWarning(RoutingWorkerLog) << "run(): generator() failed.";
} }
#endif qCWarning(RoutingWorkerLog)
#ifdef SHOW_CIRCULAR_SURVEY_TIME << "run(): execution time: "
qWarning() << "RoutingWorker::run(): execution time: " << std::chrono::duration_cast<std::chrono::milliseconds>(
<< std::chrono::duration_cast<std::chrono::milliseconds>( std::chrono::high_resolution_clock::now() - start)
std::chrono::high_resolution_clock::now() - start) .count()
.count() << " ms";
<< " ms";
#endif
// Signal calulation end and set thread to sleep. // Signal calulation end and set thread to sleep.
this->_calculating = false; this->_calculating = false;
emit calculatingChanged(); emit calculatingChanged();
...@@ -136,5 +125,5 @@ void RoutingThread::run() { ...@@ -136,5 +125,5 @@ void RoutingThread::run() {
} }
this->_restart = false; this->_restart = false;
} // main loop } // main loop
qWarning() << "RoutingWorker::run(): thread end."; qCWarning(RoutingWorkerLog) << "run(): thread end.";
} }
...@@ -48,7 +48,7 @@ public: ...@@ -48,7 +48,7 @@ public:
void setAutoPublish(bool ap); void setAutoPublish(bool ap);
void setHoldProgress(bool hp); void setHoldProgress(bool hp);
bool holdProgress(); bool lockProgress();
void publishTileData(); void publishTileData();
NemoInterface::STATUS status(); NemoInterface::STATUS status();
...@@ -88,7 +88,7 @@ private: ...@@ -88,7 +88,7 @@ private:
// Internals // Internals
std::atomic_bool running_; std::atomic_bool running_;
std::atomic_bool holdProgress_; std::atomic_bool lockProgress_;
std::atomic_bool topicServiceSetupDone; std::atomic_bool topicServiceSetupDone;
ROSBridgePtr pRosBridge; ROSBridgePtr pRosBridge;
QTimer loopTimer; QTimer loopTimer;
...@@ -110,7 +110,7 @@ StatusMap statusMap{ ...@@ -110,7 +110,7 @@ StatusMap statusMap{
NemoInterface::Impl::Impl(NemoInterface *p) NemoInterface::Impl::Impl(NemoInterface *p)
: status_(STATUS::NOT_CONNECTED), nextTimeout(TimePoint::max()), : status_(STATUS::NOT_CONNECTED), nextTimeout(TimePoint::max()),
running_(false), holdProgress_(false), topicServiceSetupDone(false), running_(false), lockProgress_(false), topicServiceSetupDone(false),
parent(p) { parent(p) {
// ROS Bridge. // ROS Bridge.
...@@ -189,11 +189,11 @@ bool NemoInterface::Impl::hasTileData(const TileData &tileData) const { ...@@ -189,11 +189,11 @@ bool NemoInterface::Impl::hasTileData(const TileData &tileData) const {
} }
void NemoInterface::Impl::setHoldProgress(bool hp) { void NemoInterface::Impl::setHoldProgress(bool hp) {
if (this->holdProgress_ != hp) { if (this->lockProgress_ != hp) {
this->holdProgress_ = hp; this->lockProgress_ = hp;
emit this->parent->holdProgressChanged(); emit this->parent->lockProgressChanged();
if (!this->holdProgress_) { if (!this->lockProgress_) {
UniqueLock lk(this->progressMutex); UniqueLock lk(this->progressMutex);
if (this->qProgress != this->qProgressHolded) { if (this->qProgress != this->qProgressHolded) {
this->qProgressHolded = this->qProgress; this->qProgressHolded = this->qProgress;
...@@ -204,7 +204,7 @@ void NemoInterface::Impl::setHoldProgress(bool hp) { ...@@ -204,7 +204,7 @@ void NemoInterface::Impl::setHoldProgress(bool hp) {
} }
} }
bool NemoInterface::Impl::holdProgress() { return this->holdProgress_.load(); } bool NemoInterface::Impl::lockProgress() { return this->lockProgress_.load(); }
void NemoInterface::Impl::publishTileData() { void NemoInterface::Impl::publishTileData() {
std::lock(this->ENUOriginMutex, this->tilesENUMutex); std::lock(this->ENUOriginMutex, this->tilesENUMutex);
...@@ -263,7 +263,7 @@ bool NemoInterface::Impl::doTopicServiceSetup() { ...@@ -263,7 +263,7 @@ bool NemoInterface::Impl::doTopicServiceSetup() {
UniqueLock lk3(this->ENUOriginMutex, std::adopt_lock); UniqueLock lk3(this->ENUOriginMutex, std::adopt_lock);
int requiredSize = this->tilesENU.polygons().size(); int requiredSize = this->tilesENU.polygons().size();
auto hold = this->holdProgress_.load(); auto hold = this->lockProgress_.load();
auto &progressMsg = this->qProgress; auto &progressMsg = this->qProgress;
if (!nemo_msgs::progress::fromJson(*pDoc, progressMsg) || if (!nemo_msgs::progress::fromJson(*pDoc, progressMsg) ||
progressMsg.progress().size() != progressMsg.progress().size() !=
...@@ -474,4 +474,4 @@ QString NemoInterface::editorQml() { ...@@ -474,4 +474,4 @@ QString NemoInterface::editorQml() {
bool NemoInterface::running() { return this->pImpl->running(); } bool NemoInterface::running() { return this->pImpl->running(); }
bool NemoInterface::holdProgress() { return this->pImpl->holdProgress(); } bool NemoInterface::lockProgress() { return this->pImpl->lockProgress(); }
...@@ -29,8 +29,8 @@ public: ...@@ -29,8 +29,8 @@ public:
Q_PROPERTY(QVector<int> progress READ progress NOTIFY progressChanged) Q_PROPERTY(QVector<int> progress READ progress NOTIFY progressChanged)
Q_PROPERTY(QString editorQml READ editorQml CONSTANT) Q_PROPERTY(QString editorQml READ editorQml CONSTANT)
Q_PROPERTY(bool running READ running NOTIFY runningChanged) Q_PROPERTY(bool running READ running NOTIFY runningChanged)
Q_PROPERTY(bool holdProgress READ holdProgress WRITE setHoldProgress NOTIFY Q_PROPERTY(bool lockProgress READ lockProgress WRITE setHoldProgress NOTIFY
holdProgressChanged) lockProgressChanged)
Q_INVOKABLE void start(); Q_INVOKABLE void start();
Q_INVOKABLE void stop(); Q_INVOKABLE void stop();
...@@ -48,13 +48,13 @@ public: ...@@ -48,13 +48,13 @@ public:
QVector<int> progress() const; QVector<int> progress() const;
QString editorQml(); QString editorQml();
bool running(); bool running();
bool holdProgress(); bool lockProgress();
signals: signals:
void statusChanged(); void statusChanged();
void progressChanged(); void progressChanged();
void runningChanged(); void runningChanged();
void holdProgressChanged(); void lockProgressChanged();
private: private:
PImpl pImpl; PImpl pImpl;
......
#include "StateMachine.h"
#include "QGCLoggingCategory.h"
#include <QDebug>
const QLoggingCategory &WimaPlanerLog();
namespace wima_planer_detail {
template <typename T>
constexpr typename std::underlying_type<T>::type integral(T value) {
return static_cast<typename std::underlying_type<T>::type>(value);
}
StateMachine::StateMachine(QObject *parent)
: QObject(parent), _state(STATE::NEEDS_INIT) {}
STATE StateMachine::state() { return this->_state; }
void StateMachine::updateState(EVENT e) {
qCDebug(WimaPlanerLog) << "StateMachine::updateState(): event:" << e;
switch (this->_state) {
case STATE::NEEDS_INIT:
switch (e) {
case EVENT::INIT_DONE:
setState(STATE::NEEDS_J_AREA_UPDATE);
break;
case EVENT::M_AREA_PATH_CHANGED:
case EVENT::S_AREA_PATH_CHANGED:
case EVENT::CORRIDOR_PATH_CHANGED:
case EVENT::M_AREA_TILES_CHANGED:
case EVENT::M_AREA_PROGRESS_CHANGED:
case EVENT::J_AREA_UPDATED:
case EVENT::DEPOT_CHANGED:
case EVENT::SURVEY_DESTROYED:
case EVENT::SURVEY_UPDATE_TRIGGERED:
case EVENT::SURVEY_UPDATED:
case EVENT::PATH_UPDATED:
break;
default:
qCCritical(WimaPlanerLog)
<< "StateMachine::updateState: Unknown event: " << e;
Q_ASSERT(false);
break;
}
break; // STATE::NEEDS_INIT
case STATE::NEEDS_J_AREA_UPDATE:
switch (e) {
case EVENT::INIT_DONE:
case EVENT::M_AREA_PATH_CHANGED:
case EVENT::S_AREA_PATH_CHANGED:
case EVENT::CORRIDOR_PATH_CHANGED:
case EVENT::M_AREA_TILES_CHANGED:
case EVENT::M_AREA_PROGRESS_CHANGED:
break;
case EVENT::J_AREA_UPDATED:
setState(STATE::NEEDS_SURVEY_UPDATE);
case EVENT::DEPOT_CHANGED:
case EVENT::SURVEY_DESTROYED:
case EVENT::SURVEY_UPDATE_TRIGGERED:
case EVENT::SURVEY_UPDATED:
case EVENT::PATH_UPDATED:
break;
default:
qCCritical(WimaPlanerLog)
<< "StateMachine::updateState: Unknown event: " << e;
Q_ASSERT(false);
break;
}
break; // STATE::NEEDS_J_AREA_UPDATE
case STATE::NEEDS_SURVEY_UPDATE:
switch (e) {
case EVENT::INIT_DONE:
case EVENT::M_AREA_PATH_CHANGED:
case EVENT::S_AREA_PATH_CHANGED:
case EVENT::CORRIDOR_PATH_CHANGED:
setState(STATE::NEEDS_J_AREA_UPDATE);
break;
case EVENT::M_AREA_TILES_CHANGED:
case EVENT::M_AREA_PROGRESS_CHANGED:
case EVENT::J_AREA_UPDATED:
case EVENT::DEPOT_CHANGED:
case EVENT::SURVEY_DESTROYED:
break;
case EVENT::SURVEY_UPDATE_TRIGGERED:
setState(STATE::WAITING_FOR_SURVEY_UPDATE);
break;
case EVENT::SURVEY_UPDATED:
case EVENT::PATH_UPDATED:
break;
default:
qCCritical(WimaPlanerLog)
<< "StateMachine::updateState: Unknown event: " << e;
Q_ASSERT(false);
break;
}
break; // STATE::NEEDS_SURVEY_UPDATE
case STATE::WAITING_FOR_SURVEY_UPDATE:
switch (e) {
case EVENT::INIT_DONE:
case EVENT::M_AREA_PATH_CHANGED:
case EVENT::S_AREA_PATH_CHANGED:
case EVENT::CORRIDOR_PATH_CHANGED:
setState(STATE::NEEDS_J_AREA_UPDATE);
break;
case EVENT::M_AREA_TILES_CHANGED:
case EVENT::M_AREA_PROGRESS_CHANGED:
case EVENT::J_AREA_UPDATED:
case EVENT::DEPOT_CHANGED:
case EVENT::SURVEY_DESTROYED:
setState(STATE::NEEDS_SURVEY_UPDATE);
break;
case EVENT::SURVEY_UPDATE_TRIGGERED:
break;
case EVENT::SURVEY_UPDATED:
setState(STATE::NEEDS_PATH_UPDATE);
case EVENT::PATH_UPDATED:
break;
default:
qCCritical(WimaPlanerLog)
<< "StateMachine::updateState: Unknown event: " << e;
Q_ASSERT(false);
break;
}
break; // STATE::WAYTING_FOR_SURVEY_UPDATE
case STATE::NEEDS_PATH_UPDATE:
switch (e) {
case EVENT::INIT_DONE:
case EVENT::M_AREA_PATH_CHANGED:
case EVENT::S_AREA_PATH_CHANGED:
case EVENT::CORRIDOR_PATH_CHANGED:
setState(STATE::NEEDS_J_AREA_UPDATE);
break;
case EVENT::M_AREA_TILES_CHANGED:
case EVENT::M_AREA_PROGRESS_CHANGED:
case EVENT::J_AREA_UPDATED:
case EVENT::DEPOT_CHANGED:
case EVENT::SURVEY_DESTROYED:
setState(STATE::NEEDS_SURVEY_UPDATE);
break;
case EVENT::SURVEY_UPDATE_TRIGGERED:
setState(STATE::WAITING_FOR_SURVEY_UPDATE);
break;
case EVENT::SURVEY_UPDATED:
break;
case EVENT::PATH_UPDATED:
setState(STATE::UP_TO_DATE);
break;
default:
qCCritical(WimaPlanerLog)
<< "StateMachine::updateState: Unknown event: " << e;
Q_ASSERT(false);
break;
}
break; // STATE::NEEDS_PATH_UPDATE
case STATE::UP_TO_DATE:
switch (e) {
case EVENT::INIT_DONE:
case EVENT::M_AREA_PATH_CHANGED:
case EVENT::S_AREA_PATH_CHANGED:
case EVENT::CORRIDOR_PATH_CHANGED:
setState(STATE::NEEDS_J_AREA_UPDATE);
break;
case EVENT::M_AREA_TILES_CHANGED:
case EVENT::M_AREA_PROGRESS_CHANGED:
case EVENT::J_AREA_UPDATED:
case EVENT::DEPOT_CHANGED:
case EVENT::SURVEY_DESTROYED:
setState(STATE::NEEDS_SURVEY_UPDATE);
break;
case EVENT::SURVEY_UPDATE_TRIGGERED:
setState(STATE::WAITING_FOR_SURVEY_UPDATE);
break;
case EVENT::SURVEY_UPDATED:
setState(STATE::NEEDS_PATH_UPDATE);
break;
case EVENT::PATH_UPDATED:
break;
default:
qCCritical(WimaPlanerLog)
<< "StateMachine::updateState: Unknown event: " << e;
Q_ASSERT(false);
break;
}
break; // STATE::UP_TO_DATE
default:
qCCritical(WimaPlanerLog)
<< "StateMachine::updateState: Unknown state: " << this->_state;
Q_ASSERT(false);
break;
}
}
bool StateMachine::upToDate() { return this->_state == STATE::UP_TO_DATE; }
void StateMachine::setState(STATE s) {
if (this->_state != s) {
auto oldState = this->_state;
this->_state = s;
emit stateChanged();
if (oldState == STATE::UP_TO_DATE || s == STATE::UP_TO_DATE) {
emit upToDateChanged();
}
qCDebug(WimaPlanerLog) << "StateMachine::setState():" << oldState << "->"
<< s;
}
}
QDebug &operator<<(QDebug &ds, STATE s) {
switch (s) {
case STATE::NEEDS_INIT:
ds << "NEEDS_INIT";
break;
case STATE::NEEDS_J_AREA_UPDATE:
ds << "NEEDS_J_AREA_UPDATE";
break;
case STATE::NEEDS_SURVEY_UPDATE:
ds << "NEEDS_SURVEY_UPDATE";
break;
case STATE::WAITING_FOR_SURVEY_UPDATE:
ds << "WAITING_FOR_SURVEY_UPDATE";
break;
case STATE::NEEDS_PATH_UPDATE:
ds << "NEEDS_PATH_UPDATE";
break;
case STATE::UP_TO_DATE:
ds << "UP_TO_DATE";
break;
}
return ds;
}
QDebug &operator<<(QDebug &ds, EVENT s) {
switch (s) {
case EVENT::INIT_DONE:
ds << "INIT_DONE";
break;
case EVENT::M_AREA_PATH_CHANGED:
ds << "M_AREA_PATH_CHANGED";
break;
case EVENT::S_AREA_PATH_CHANGED:
ds << "S_AREA_PATH_CHANGED";
break;
case EVENT::CORRIDOR_PATH_CHANGED:
ds << "CORRIDOR_PATH_CHANGED";
break;
case EVENT::M_AREA_TILES_CHANGED:
ds << "M_AREA_TILES_CHANGED";
break;
case EVENT::M_AREA_PROGRESS_CHANGED:
ds << "M_AREA_PROGRESS_CHANGED";
break;
case EVENT::J_AREA_UPDATED:
ds << "J_AREA_UPDATED";
break;
case EVENT::DEPOT_CHANGED:
ds << "DEPOT_CHANGED";
break;
case EVENT::SURVEY_DESTROYED:
ds << "SURVEY_DESTROYED";
break;
case EVENT::SURVEY_UPDATE_TRIGGERED:
ds << "SURVEY_UPDATE_TRIGGERED";
break;
case EVENT::SURVEY_UPDATED:
ds << "SURVEY_UPDATED";
break;
case EVENT::PATH_UPDATED:
ds << "PATH_UPDATED";
break;
}
return ds;
}
} // namespace wima_planer_detail
#pragma once
#include <QDataStream>
#include <QObject>
namespace wima_planer_detail {
enum class STATE {
NEEDS_INIT,
NEEDS_J_AREA_UPDATE,
NEEDS_SURVEY_UPDATE,
WAITING_FOR_SURVEY_UPDATE,
NEEDS_PATH_UPDATE,
UP_TO_DATE
};
QDebug &operator<<(QDebug &ds, STATE s);
enum class EVENT {
INIT_DONE,
M_AREA_PATH_CHANGED,
S_AREA_PATH_CHANGED,
CORRIDOR_PATH_CHANGED,
M_AREA_TILES_CHANGED,
M_AREA_PROGRESS_CHANGED,
J_AREA_UPDATED,
DEPOT_CHANGED,
SURVEY_DESTROYED,
SURVEY_UPDATE_TRIGGERED,
SURVEY_UPDATED,
PATH_UPDATED,
};
QDebug &operator<<(QDebug &ds, EVENT s);
class StateMachine : public QObject {
Q_OBJECT
public:
explicit StateMachine(QObject *parent = nullptr);
STATE state();
void updateState(EVENT e);
bool upToDate();
signals:
void stateChanged();
void upToDateChanged();
private:
void setState(STATE s);
STATE _state;
};
} // namespace wima_planer_detail
This diff is collapsed.
...@@ -2,6 +2,7 @@ ...@@ -2,6 +2,7 @@
#include "QmlObjectListModel.h" #include "QmlObjectListModel.h"
#include <QObject> #include <QObject>
#include <QScopedPointer>
#include <QSharedPointer> #include <QSharedPointer>
#include "Geometry/WimaCorridor.h" #include "Geometry/WimaCorridor.h"
...@@ -22,6 +23,10 @@ class MissionController; ...@@ -22,6 +23,10 @@ class MissionController;
class PlanMasterController; class PlanMasterController;
class WimaBridge; class WimaBridge;
namespace wima_planer_detail {
class StateMachine;
}
class WimaPlaner : public QObject { class WimaPlaner : public QObject {
Q_OBJECT Q_OBJECT
...@@ -29,6 +34,7 @@ class WimaPlaner : public QObject { ...@@ -29,6 +34,7 @@ class WimaPlaner : public QObject {
public: public:
WimaPlaner(QObject *parent = nullptr); WimaPlaner(QObject *parent = nullptr);
~WimaPlaner();
template <class T> WimaPlaner(T t, QObject *parent = nullptr) = delete; template <class T> WimaPlaner(T t, QObject *parent = nullptr) = delete;
template <class T> WimaPlaner(T t) = delete; template <class T> WimaPlaner(T t) = delete;
...@@ -85,10 +91,9 @@ public: ...@@ -85,10 +91,9 @@ public:
/// MissionController /// MissionController
Q_INVOKABLE void removeAll(); Q_INVOKABLE void removeAll();
/// Recalculates vehicle corridor, flight path, etc. /// Recalculates vehicle corridor, flight path, etc.
Q_INVOKABLE bool update(); Q_INVOKABLE void update();
/// Pushes the generated mission data to the wimaController. /// Pushes the generated mission data to the wimaController.
Q_INVOKABLE void synchronize(); Q_INVOKABLE void synchronize();
Q_INVOKABLE bool readyForSynchronization();
Q_INVOKABLE void saveToCurrent(); Q_INVOKABLE void saveToCurrent();
Q_INVOKABLE void saveToFile(const QString &filename); Q_INVOKABLE void saveToFile(const QString &filename);
Q_INVOKABLE bool loadFromCurrent(); Q_INVOKABLE bool loadFromCurrent();
...@@ -114,6 +119,17 @@ signals: ...@@ -114,6 +119,17 @@ signals:
private slots: private slots:
void updatePolygonInteractivity(int index); void updatePolygonInteractivity(int index);
void _update();
void CSDestroyedHandler();
void CSMissionItemReadyHandler();
void CSCalculatingChangedHandler();
void mAreaPathChangedHandler();
void mAreaTilesChangedHandler();
void mAreaProgressChangedHandler();
void sAreaPathChangedHandler();
void corridorPathChangedHandler();
void depotChangedHandler();
#ifndef NDEBUG #ifndef NDEBUG
void autoLoadMission(void); void autoLoadMission(void);
#endif #endif
...@@ -121,6 +137,7 @@ private slots: ...@@ -121,6 +137,7 @@ private slots:
private: private:
signals: signals:
void joinedAreaValidChanged(); void joinedAreaValidChanged();
void stateChanged();
private: private:
// Member Functions // Member Functions
...@@ -128,7 +145,9 @@ private: ...@@ -128,7 +145,9 @@ private:
bool shortestPath(const QGeoCoordinate &start, bool shortestPath(const QGeoCoordinate &start,
const QGeoCoordinate &destination, const QGeoCoordinate &destination,
QVector<QGeoCoordinate> &path); QVector<QGeoCoordinate> &path);
void setNeedsUpdate(bool needsUpdate); void setSynchronized(bool s);
void enableMonitoring();
void disableMonitoring();
// Member Variables // Member Variables
PlanMasterController *_masterController; PlanMasterController *_masterController;
...@@ -140,9 +159,9 @@ private: ...@@ -140,9 +159,9 @@ private:
bool _joinedAreaValid; bool _joinedAreaValid;
WimaMeasurementArea _measurementArea; WimaMeasurementArea _measurementArea;
bool _mAreaChanged; bool _copyMAreaToSurvey;
WimaServiceArea _serviceArea; WimaServiceArea _serviceArea;
bool _sAreaChanged; bool _copySAreaToSurvey;
WimaCorridor _corridor; WimaCorridor _corridor;
bool _corridorChanged; bool _corridorChanged;
// contains all visible areas // contains all visible areas
...@@ -154,12 +173,11 @@ private: ...@@ -154,12 +173,11 @@ private:
// path from last measurement point to land // path from last measurement point to land
unsigned long _returnPathLength; unsigned long _returnPathLength;
CircularSurvey *_TSComplexItem; CircularSurvey *_survey;
bool _surveyChanged; bool _surveyChanged;
// sync stuff // sync stuff
bool _synchronized; // true if planData is synchronized with bool _synchronized; // true if planData is synchronized with
// wimaController // wimaController
bool _needsUpdate; // gets set by updateMission and calcArrivalAndReturnPath
#ifndef NDEBUG #ifndef NDEBUG
QTimer _autoLoadTimer; // timer to auto load mission after some time, prevents QTimer _autoLoadTimer; // timer to auto load mission after some time, prevents
...@@ -167,4 +185,7 @@ private: ...@@ -167,4 +185,7 @@ private:
#endif #endif
NemoInterface _nemoInterface; NemoInterface _nemoInterface;
// State
QScopedPointer<wima_planer_detail::StateMachine> _stateMachine;
}; };
...@@ -65,6 +65,7 @@ Item { ...@@ -65,6 +65,7 @@ Item {
function _destroyEntryCoordinate(){ function _destroyEntryCoordinate(){
if (_entryCoordinate){ if (_entryCoordinate){
map.removeMapItem(_entryCoordinate)
_entryCoordinate.destroy() _entryCoordinate.destroy()
_entryCoordinate = undefined _entryCoordinate = undefined
} }
...@@ -72,6 +73,7 @@ Item { ...@@ -72,6 +73,7 @@ Item {
function _destroyExitCoordinate(){ function _destroyExitCoordinate(){
if (_exitCoordinate){ if (_exitCoordinate){
map.removeMapItem(_exitCoordinate)
_exitCoordinate.destroy() _exitCoordinate.destroy()
_exitCoordinate = undefined _exitCoordinate = undefined
} }
...@@ -79,6 +81,7 @@ Item { ...@@ -79,6 +81,7 @@ Item {
function _destroyRefPoint(){ function _destroyRefPoint(){
if (_refPoint){ if (_refPoint){
map.removeMapItem(_refPoint)
_refPoint.destroy() _refPoint.destroy()
_refPoint = undefined _refPoint = undefined
} }
...@@ -86,6 +89,7 @@ Item { ...@@ -86,6 +89,7 @@ Item {
function _destroyTransectsComponent(){ function _destroyTransectsComponent(){
if (_transectsComponent){ if (_transectsComponent){
map.removeMapItem(_transectsComponent)
_transectsComponent.destroy() _transectsComponent.destroy()
_transectsComponent = undefined _transectsComponent = undefined
} }
......
...@@ -252,7 +252,7 @@ Rectangle { ...@@ -252,7 +252,7 @@ Rectangle {
rowSpacing: _rowSpacing rowSpacing: _rowSpacing
columnSpacing: _labelToValueSpacing columnSpacing: _labelToValueSpacing
Layout.alignment: Qt.AlignVCenter | Qt.AlignHCenter Layout.alignment: Qt.AlignVCenter | Qt.AlignHCenter
// visible: _batteryInfoAvailable visible: _batteryInfoAvailable
QGCLabel { QGCLabel {
text: qsTr("Battery") text: qsTr("Battery")
......
...@@ -966,6 +966,12 @@ QGCView { ...@@ -966,6 +966,12 @@ QGCView {
checked: true checked: true
visible: QGroundControl.corePlugin.options.enablePlanViewSelector visible: QGroundControl.corePlugin.options.enablePlanViewSelector
anchors.verticalCenter: parent.verticalCenter anchors.verticalCenter: parent.verticalCenter
onCheckedChanged:{
if (!checked){
wimaPlaner.update()
}
}
} }
QGCRadioButton { QGCRadioButton {
id: planElementMission id: planElementMission
......
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