Commit 22c1b55e authored by Valentin Platzgummer's avatar Valentin Platzgummer

temp

parent 5beee98a
......@@ -35,7 +35,7 @@ DebugBuild {
#DEFINES += SNAKE_SHOW_TIME
#DEFINES += SNAKE_DEBUG
#DEFINES += SNAKE_SHOW_TIME
DEFINES += ROS_BRIDGE_DEBUG
#DEFINES += ROS_BRIDGE_DEBUG
}
else {
DESTDIR = $${OUT_PWD}/release
......@@ -453,10 +453,12 @@ HEADERS += \
src/MeasurementComplexItem/geometry/TileDiff.h \
src/MeasurementComplexItem/geometry/geometry.h \
src/MeasurementComplexItem/HashFunctions.h \
src/MeasurementComplexItem/nemo_interface/FutureWatcher.h \
src/MeasurementComplexItem/nemo_interface/FutureWatcherInterface.h \
src/MeasurementComplexItem/nemo_interface/Task.h \
src/MeasurementComplexItem/nemo_interface/TaskDispatcher.h \
src/MeasurementComplexItem/nemo_interface/tileHelper.h \
src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h \
src/MeasurementComplexItem/nemo_interface/CommandDispatcher.h \
src/MeasurementComplexItem/nemo_interface/MeasurementTile.h \
src/QmlControls/QmlUnitsConversion.h \
src/MeasurementComplexItem/geometry/GeoArea.h \
......@@ -510,6 +512,7 @@ HEADERS += \
src/comm/ros_bridge/include/message_traits.h \
src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h \
src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h \
src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.h \
src/comm/ros_bridge/include/messages/nemo_msgs/tile.h \
src/comm/ros_bridge/include/messages/std_msgs/header.h \
src/comm/ros_bridge/include/server.h \
......@@ -529,9 +532,10 @@ SOURCES += \
src/MeasurementComplexItem/geometry/SafeArea.cc \
src/MeasurementComplexItem/geometry/geometry.cpp \
src/MeasurementComplexItem/HashFunctions.cpp \
src/MeasurementComplexItem/nemo_interface/CommandDispatcher.cpp \
src/MeasurementComplexItem/nemo_interface/FutureWatcherInterface.cpp \
src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp \
src/MeasurementComplexItem/nemo_interface/Task.cpp \
src/MeasurementComplexItem/nemo_interface/TaskDispatcher.cpp \
src/Vehicle/VehicleEscStatusFactGroup.cc \
src/MeasurementComplexItem/AreaData.cc \
src/api/QGCCorePlugin.cc \
......@@ -553,6 +557,7 @@ SOURCES += \
src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.cpp \
src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.cpp \
src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.cpp \
src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.cpp \
src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp \
src/comm/ros_bridge/include/messages/std_msgs/header.cpp \
src/comm/ros_bridge/include/messages/std_msgs/time.cpp \
......
......@@ -2,6 +2,7 @@
#include "CircularGenerator.h"
#include "LinearGenerator.h"
#include "NemoInterface.h"
#include "RoutingThread.h"
#include "geometry/GenericCircle.h"
#include "geometry/MeasurementArea.h"
......@@ -128,6 +129,10 @@ MeasurementComplexItem::MeasurementComplexItem(
resetGenerators();
startEditing();
// connect to nemo interface
connect(pNemoInterface, &NemoInterface::progressChanged, this,
&MeasurementComplexItem::_onNewProgress);
}
MeasurementComplexItem::~MeasurementComplexItem() {}
......@@ -919,6 +924,69 @@ void MeasurementComplexItem::_reverseRoute() {
}
}
void MeasurementComplexItem::_syncTiles() {
auto areaArray = _pAreaData->measurementAreaArray();
bool clear = false;
if (areaArray.size() > 0) {
// create tile ptr array
TilePtrArray tilePtrArray;
auto *pMeasurementArea = areaArray[0];
auto pTiles = pMeasurementArea->tiles();
for (int i = 0; i < pTiles->count(); ++i) {
auto *tile = pTiles->value<MeasurementTile *>(i);
Q_ASSERT(tile != nullptr);
tilePtrArray.push_back(tile);
}
if (tilePtrArray.size() > 0) {
// create id array
IDArray idArray;
for (const auto *pTile : tilePtrArray) {
idArray.push_back(pTile->id());
}
// sync. necessary?
bool doSync = false;
auto contains = pNemoInterface->containsTiles(idArray);
for (auto &&logical : contains) {
if (logical == false) {
doSync = true;
break;
}
}
if (doSync) {
if (!pNemoInterface->empty()) {
(void)pNemoInterface->clearTiles();
}
(void)pNemoInterface->addTiles(tilePtrArray);
return;
}
} else {
clear = true;
}
} else {
clear = true;
}
if (clear) {
if (!pNemoInterface->empty()) {
(void)pNemoInterface->clearTiles();
}
}
}
void MeasurementComplexItem::_onNewProgress(const ProgressArray &array) {
auto areaArray = this->_pAreaData->measurementAreaArray();
if (areaArray.size() > 0) {
for (auto &area : areaArray) {
area->updateProgress(array);
}
}
}
ComplexMissionItem::ReadyForSaveState
MeasurementComplexItem::readyForSaveState() const {
if (idle()) {
......@@ -1145,6 +1213,10 @@ bool MeasurementComplexItem::stopEditing(bool doUpdate) {
_updateRoute();
}
if (correct && isDifferent) {
_syncTiles();
}
return updated;
}
return false;
......@@ -1153,6 +1225,7 @@ bool MeasurementComplexItem::stopEditing(bool doUpdate) {
void MeasurementComplexItem::abortEditing() {
if (editing()) {
_setAreaData(_pAreaData);
_syncTiles();
_setState(STATE::IDLE);
}
}
......
......@@ -10,6 +10,7 @@
#include "SettingsFact.h"
#include "AreaData.h"
#include "ProgressArray.h"
class RoutingThread;
class RoutingResult;
......@@ -238,6 +239,8 @@ private slots:
void _updateRoute();
void _changeVariantIndex();
void _reverseRoute();
void _syncTiles();
void _onNewProgress(const ProgressArray &array);
private:
bool _setGenerator(PtrGenerator newG);
......
......@@ -2,7 +2,9 @@
#include <QGeoCoordinate>
#include <QObject>
#include <QVariant>
#include <future>
#include <memory>
#include "IDArray.h"
......@@ -28,10 +30,10 @@ public:
enum class STATUS {
NOT_CONNECTED,
SYNC,
READY,
WEBSOCKET_DETECTED,
TIMEOUT,
INVALID_HEARTBEAT
};
Q_ENUM(STATUS)
......@@ -49,9 +51,28 @@ public:
Q_INVOKABLE void stop();
// Tile editing.
void addTiles(const TileArray &tileArray);
void removeTiles(const IDArray &idArray);
void clearTiles();
//!
//! \brief addTiles
//! \param tileArray
//! \return Returns a QVariant containing a boolean value, indicating if the
//! requested operation was successfull.
//!
std::shared_future<QVariant> addTiles(const TileArray &tileArray);
std::shared_future<QVariant> addTiles(const TilePtrArray &tileArray);
//!
//! \brief removeTiles
//! \param idArray
//! \return Returns a QVariant containing a boolean value, indicating if the
//! requested operation was successfull.
//!
std::shared_future<QVariant> removeTiles(const IDArray &idArray);
//!
//! \brief clearTiles
//! \return Returns a QVariant containing a boolean value, indicating if the
//! requested operation was successfull.
//!
std::shared_future<QVariant> clearTiles();
TileArray getTiles(const IDArray &idArray);
TileArray getAllTiles();
LogicalArray containsTiles(const IDArray &idArray);
......
......@@ -82,20 +82,7 @@ bool GeoArea::covers(const QGeoCoordinate &c) {
}
}
void GeoArea::init() {
this->setObjectName(nameString);
// connect(this, &GeoArea::pathChanged, [this] {
// if (this->objectName() != "Tile") {
// qDebug() << this->objectName() << " path: " << this->path() << "\n";
// }
// });
// connect(this, &GeoArea::centerChanged, [this] {
// if (this->objectName() != "Tile") {
// qDebug() << this->objectName() << " center: " << this->center() <<
// "\n";
// }
// });
}
void GeoArea::init() { this->setObjectName(nameString); }
void GeoArea::setErrorString(const QString &str) {
this->_errorString = str;
......
......@@ -33,23 +33,6 @@ namespace {
const char *tileArrayKey = "TileArray";
} // namespace
TileData::TileData() {}
TileData::~TileData() {}
TileData &TileData::operator=(const TileData &other) { return *this; }
bool TileData::operator==(const TileData &other) const { return false; }
bool TileData::operator!=(const TileData &other) const {
return operator==(other);
}
void TileData::saveToJson(QJsonObject &json) {}
bool TileData::loadFromJson(const QJsonObject &json, QString &errorString) {
return false;
}
void TileData::clear() {}
std::size_t TileData::size() const { return 0; }
const char *MeasurementArea::settingsGroup = "MeasurementArea";
const char *tileHeightKey = "TileHeight";
const char *tileWidthName = "TileWidth";
......@@ -107,7 +90,7 @@ MeasurementArea::MeasurementArea(const MeasurementArea &other, QObject *parent)
qobject_cast<const MeasurementTile *>(other._tiles->operator[](i))
->clone(_tiles.get()));
}
_tileMap = other._tileMap;
_indexMap = other._indexMap;
enableUpdate();
} else {
enableUpdate();
......@@ -131,7 +114,7 @@ MeasurementArea &MeasurementArea::operator=(const MeasurementArea &other) {
qobject_cast<const MeasurementTile *>(other._tiles->operator[](i))
->clone(_tiles.get()));
}
_tileMap = other._tileMap;
_indexMap = other._indexMap;
enableUpdate();
} else {
enableUpdate();
......@@ -270,6 +253,41 @@ bool MeasurementArea::loadFromJson(const QJsonObject &json,
break;
}
}
if (!tileError) {
this->_indexMap.clear();
for (int i = 0; i < _tiles->count(); ++i) {
auto tile = qobject_cast<MeasurementTile *>(_tiles->get(i));
auto it = _indexMap.find(tile->id());
// find unique id
if (it != _indexMap.end()) {
auto newId = tile->id() + 1;
constexpr long counterMax = 1e6;
unsigned long counter = 0;
for (; counter <= counterMax; ++counter) {
it = _indexMap.find(newId);
if (it == _indexMap.end()) {
break;
} else {
++newId;
}
}
if (counter != counterMax) {
tile->setId(newId);
tile->setProgress(0.0);
} else {
qCritical() << "MeasurementArea::storeTiles(): not able to find "
"unique id!";
continue;
}
}
_indexMap.insert(std::make_pair(tile->id(), i));
}
}
} else {
qCWarning(MeasurementAreaLog)
<< "Not able to load tiles. tileArrayKey missing or wrong type.";
......@@ -307,15 +325,22 @@ bool MeasurementArea::isCorrect() {
void MeasurementArea::updateProgress(const ProgressArray &array) {
if (ready() && !_holdProgress && array.size() > 0) {
bool anyChanges = false;
for (const auto &pair : array) {
const auto &id = pair.first;
const auto &progress = pair.second;
auto it = _tileMap.find(id);
if (it != _tileMap.end()) {
if (!qFuzzyCompare(progress, it->second->progress())) {
it->second->setProgress(progress);
long counter = 0;
for (const auto &lp : array) {
qDebug() << "MeasurementArea::updateProgress: counter = " << counter++;
auto it = _indexMap.find(lp.id());
if (it != _indexMap.end()) {
int tileIndex = it->second;
auto *tile = _tiles->value<MeasurementTile *>(tileIndex);
qDebug() << "MeasurementArea::updateProgress: progress before = "
<< tile->progress();
if (!qFuzzyCompare(lp.progress(), tile->progress())) {
tile->setProgress(lp.progress());
anyChanges = true;
}
qDebug() << "MeasurementArea::updateProgress: progress after = "
<< tile->progress();
}
}
......@@ -330,6 +355,8 @@ void MeasurementArea::randomProgress() {
std::srand(std::time(nullptr));
ProgressArray progressArray;
for (int i = 0; i < _tiles->count(); ++i) {
auto tile = _tiles->value<MeasurementTile *>(i);
......@@ -341,10 +368,10 @@ void MeasurementArea::randomProgress() {
p = 100;
}
tile->setProgress(p);
progressArray.append(LabeledProgress(p, tile->id()));
}
emit progressChanged();
updateProgress(progressArray);
}
}
......@@ -421,7 +448,7 @@ void MeasurementArea::doUpdate() {
geoTile->push_back(geoVertex);
hashValue ^= hashFun(geoVertex);
}
geoTile->setId(long(hashValue));
geoTile->setId(std::int64_t(hashValue));
pData->append(geoTile);
}
}
......@@ -452,7 +479,7 @@ void MeasurementArea::deferUpdate() {
if (this->_state == STATE::IDLE || this->_state == STATE::DEFERED) {
qCDebug(MeasurementAreaLog) << "defereUpdate(): defer update.";
if (this->_state == STATE::IDLE) {
this->_tileMap.clear();
this->_indexMap.clear();
this->_tiles->clearAndDeleteContents();
emit tilesChanged();
emit progressChanged();
......@@ -478,20 +505,20 @@ void MeasurementArea::storeTiles() {
QQmlEngine::CppOwnership);
// update tileMap
this->_tileMap.clear();
this->_indexMap.clear();
for (int i = 0; i < _tiles->count(); ++i) {
auto tile = qobject_cast<MeasurementTile *>(_tiles->get(i));
auto it = _tileMap.find(tile->id());
auto it = _indexMap.find(tile->id());
// find unique id
if (it != _tileMap.end()) {
long newId = tile->id() + 1;
if (it != _indexMap.end()) {
auto newId = tile->id() + 1;
constexpr long counterMax = 1e6;
unsigned long counter = 0;
for (; counter <= counterMax; ++counter) {
it = _tileMap.find(newId);
if (it == _tileMap.end()) {
it = _indexMap.find(newId);
if (it == _indexMap.end()) {
break;
} else {
++newId;
......@@ -508,7 +535,7 @@ void MeasurementArea::storeTiles() {
}
}
_tileMap.insert(std::make_pair(tile->id(), tile));
_indexMap.insert(std::make_pair(tile->id(), i));
}
// This is expensive. Drawing tiles is expensive too.
......@@ -581,20 +608,22 @@ void MeasurementArea::setHoldProgress(bool holdProgress) {
void MeasurementArea::updateIds(const QList<TileDiff> &array) {
for (const auto &diff : array) {
auto it = _tileMap.find(diff.oldTile.id());
auto it = _indexMap.find(diff.oldTile.id());
if (it != _tileMap.end() &&
diff.oldTile.coordinateList() == it->second->coordinateList()) {
if (it != _indexMap.end()) {
int tileIndex = it->second;
auto *tile = _tiles->value<MeasurementTile *>(tileIndex);
if (diff.oldTile.coordinateList() == tile->coordinateList()) {
// Change id and update _tileMap.
const auto newId = diff.newTile.id();
auto tile = it->second;
tile->setId(newId);
_tileMap.erase(it);
auto ret = _tileMap.insert(std::make_pair(newId, tile));
_indexMap.erase(it);
auto ret = _indexMap.insert(std::make_pair(newId, tileIndex));
Q_ASSERT(ret.second == true /*insert success?*/);
Q_UNUSED(ret);
}
}
}
}
bool getTiles(const FPolygon &area, Length tileHeight, Length tileWidth,
......@@ -673,7 +702,7 @@ bool getTiles(const FPolygon &area, Length tileHeight, Length tileWidth,
boost_tiles))
continue;
for (FPolygon t : boost_tiles) {
for (FPolygon &t : boost_tiles) {
if (bg::area(t) > minTileArea.value()) {
// Transform boost_tile to world coordinate system.
FPolygon rotated_tile;
......
......@@ -54,10 +54,10 @@ public:
// Overrides from GeoArea
QString mapVisualQML(void) const override;
QString editorQML(void) const override;
MeasurementArea *clone(QObject *parent = nullptr) const;
MeasurementArea *clone(QObject *parent = nullptr) const override;
bool saveToJson(QJsonObject &json) override;
bool loadFromJson(const QJsonObject &json, QString &errorString) override;
Q_INVOKABLE virtual bool isCorrect();
Q_INVOKABLE virtual bool isCorrect() override;
// Property getters.
Fact *tileHeight();
......@@ -122,7 +122,7 @@ private:
// Tile stuff.
TilePtr _tiles;
std::map<long /*id*/, MeasurementTile *> _tileMap;
std::map<std::int64_t /*id*/, int> _indexMap;
bool _holdProgress;
QTimer _timer;
STATE _state;
......
......@@ -4,7 +4,10 @@
#include <QVector>
#include <tuple>
typedef std::pair<long /*id*/, double /*progress*/> TaggedProgress;
typedef QVector<TaggedProgress> ProgressArray;
#include "comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h"
typedef ros_bridge::messages::nemo_msgs::labeled_progress::LabeledProgress
LabeledProgress;
typedef QVector<LabeledProgress> ProgressArray;
#endif // PROGRESSARRAY_H
#ifndef COMMANDDISPATCHER_H
#define COMMANDDISPATCHER_H
#include <QThread>
#include <QVariant>
#include "Task.h"
namespace nemo_interface {
class CommandDispatcher : public QThread {
public:
CommandDispatcher();
bool interruptionPoint(); // thread safe
std::future<QVariant> dispatch(const Task &c); // thread safe
std::future<QVariant> dispatchNext(const Task &c); // thread safe
void clear(); // thread safe
void stop(); // thread safe
bool running(); // thread safe
private:
std::atomic_bool _running;
std::condition_variable _condVar;
QList<Task> _queue;
std::mutex _queueMutex;
};
} // namespace nemo_interface
#endif // COMMANDDISPATCHER_H
#ifndef FUTUREWATCHER_H
#define FUTUREWATCHER_H
#include "FutureWatcherInterface.h"
#include <QTimer>
#include <future>
namespace nemo_interface {
//!
//! class similar to QFutureWatcher suitable for a std::future or
//! std::shared_future. \note For single thread use only.
//!
template <class T, template <class> class Future = std::future>
class FutureWatcher : public FutureWatcherInterface {
enum class STATE { EMPTY, STARTED, FINISHED };
public:
typedef Future<T> FutureType;
explicit FutureWatcher(QObject *parent = nullptr)
: FutureWatcherInterface(parent), _state(STATE::EMPTY) {
_init();
}
FutureWatcher(const FutureType &f, QObject *parent = nullptr)
: FutureWatcherInterface(parent), _future(f), _state(STATE::STARTED) {
_init();
}
FutureType future() const { return _future; }
void setFuture(const FutureType &future) {
_future = future;
_state = STATE::STARTED;
_timer.start(1);
emit started();
}
T result() { return _future.get(); }
virtual void waitForFinished() override {
if (_state == STATE::STARTED) {
_timer.stop();
_future.wait();
_state = STATE::FINISHED;
}
}
virtual bool isStarted() override { return _state == STATE::STARTED; }
virtual bool isFinished() override { return _state == STATE::FINISHED; }
private:
void _onTimeout() {
if (_state == STATE::STARTED) {
auto status = _future.wait_for(std::chrono::seconds(0));
if (status == std::future_status::ready) {
_state = STATE::FINISHED;
emit finished();
}
}
}
void _init() {
connect(&_timer, &QTimer::timeout, [this] { this->_onTimeout(); });
}
FutureType _future;
QTimer _timer;
STATE _state;
};
} // namespace nemo_interface
#endif // FUTUREWATCHER_H
#include "CommandDispatcher.h"
#include "FutureWatcherInterface.h"
namespace nemo_interface {
CommandDispatcher::CommandDispatcher() {}
FutureWatcherInterface::FutureWatcherInterface(QObject *parent)
: QObject(parent) {}
} // namespace nemo_interface
#ifndef FUTUREWATCHERINTERFACE_H
#define FUTUREWATCHERINTERFACE_H
#include <QObject>
namespace nemo_interface {
class FutureWatcherInterface : public QObject {
Q_OBJECT
public:
FutureWatcherInterface(QObject *parent = nullptr);
virtual void waitForFinished() = 0;
virtual bool isStarted() = 0;
virtual bool isFinished() = 0;
signals:
void finished();
void started();
};
} // namespace nemo_interface
#endif // FUTUREWATCHERINTERFACE_H
#include "MeasurementTile.h"
#include <QDebug>
const char *MeasurementTile::settingsGroup = "MeasurementTile";
const char *MeasurementTile::nameString = "MeasurementTile";
MeasurementTile::MeasurementTile(QObject *parent)
: GeoArea(parent), _progress(0), _id(0) {
init();
......@@ -31,7 +36,7 @@ void MeasurementTile::push_back(const QGeoCoordinate &c) {
this->appendVertex(c);
}
void MeasurementTile::init() { this->setObjectName("Tile"); }
void MeasurementTile::init() { this->setObjectName(nameString); }
int64_t MeasurementTile::id() const { return _id; }
......
......@@ -17,7 +17,7 @@ public:
virtual QString mapVisualQML() const override;
virtual QString editorQML() const override;
virtual MeasurementTile *clone(QObject *parent) const;
virtual MeasurementTile *clone(QObject *parent) const override;
void push_back(const QGeoCoordinate &c);
......@@ -29,6 +29,10 @@ public:
QList<QGeoCoordinate> tile() const;
// Static Variables
static const char *settingsGroup;
static const char *nameString;
signals:
void progressChanged();
void idChanged();
......
......@@ -2,10 +2,14 @@
namespace nemo_interface {