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