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,18 +608,20 @@ void MeasurementArea::setHoldProgress(bool holdProgress) {
void MeasurementArea::updateIds(const QList<TileDiff> &array) {
for (const auto &diff : array) {
auto it = _tileMap.find(diff.oldTile.id());
if (it != _tileMap.end() &&
diff.oldTile.coordinateList() == it->second->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));
Q_ASSERT(ret.second == true /*insert success?*/);
Q_UNUSED(ret);
auto it = _indexMap.find(diff.oldTile.id());
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();
tile->setId(newId);
_indexMap.erase(it);
auto ret = _indexMap.insert(std::make_pair(newId, tileIndex));
Q_ASSERT(ret.second == true /*insert success?*/);
Q_UNUSED(ret);
}
}
}
}
......@@ -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 {
Task::Task(const Task::Functor &onExec)
: _onExec(onExec), _isExpired([] { return false; }),
_isReady([] { return true; }) {}
Task::Task(const Task::Functor &onExec) : _onExec(onExec) {}
void Task::exec(std::promise<QVariant> p) { this->_onExec(std::move(p)); }
Task::Task(Task::Functor &&onExec) : _onExec(std::move(onExec)) {}
QVariant Task::exec() { return this->_onExec(); }
void Task::setOnExec(const Task::Functor &onExec) { _onExec = onExec; }
void Task::setOnExec(Task::Functor &&onExec) { _onExec = std::move(onExec); }
} // namespace nemo_interface
......@@ -10,23 +10,17 @@ namespace nemo_interface {
class Task {
public:
typedef std::function<void(std::promise<QVariant> p)> Functor;
typedef std::function<bool()> BooleanFunctor;
typedef std::function<QVariant(void)> Functor;
Task(const Functor &onExec);
Task(Functor &&onExec);
void exec(std::promise<QVariant> p);
bool isReady(); // default == true
bool isExpired(); // default == false
QVariant exec();
void setOnExec(const Functor &onExec);
void setIsReady(const BooleanFunctor &isReady);
void setIsExpired(const BooleanFunctor &isExpired);
void setOnExec(Functor &&onExec);
private:
Functor _onExec;
BooleanFunctor _isExpired;
BooleanFunctor _isReady;
};
} // namespace nemo_interface
......
#include "TaskDispatcher.h"
#include "QtConcurrent"
typedef std::unique_lock<std::mutex> ULock;
namespace nemo_interface {
TaskDispatcher::TaskDispatcher() : _running(false) {}
TaskDispatcher::~TaskDispatcher() { stop(); }
std::future<QVariant> TaskDispatcher::dispatch(std::unique_ptr<Task> c) {
ULock lk1(this->_mutex);
this->_taskQueue.push_back(std::move(c));
std::promise<QVariant> promise;
auto future = promise.get_future();
this->_promiseQueue.push_back(std::move(promise));
if (!this->_running) {
lk1.unlock();
this->start();
}
return future;
}
std::future<QVariant> TaskDispatcher::dispatchNext(std::unique_ptr<Task> c) {
ULock lk1(this->_mutex);
this->_taskQueue.push_front(std::move(c));
std::promise<QVariant> promise;
auto future = promise.get_future();
this->_promiseQueue.push_front(std::move(promise));
if (!this->_running) {
lk1.unlock();
this->start();
}
return future;
}
void TaskDispatcher::clear() {
ULock lk(this->_mutex);
this->_taskQueue.clear();
this->_promiseQueue.clear();
}
void TaskDispatcher::start() {
ULock lk1(this->_mutex);
if (!_running) {
this->_running = true;
lk1.unlock();
_future = QtConcurrent::run([this]() mutable { return this->run(); });
}
}
void TaskDispatcher::stop() {
ULock lk1(this->_mutex);
if (_running) {
this->_running = false;
lk1.unlock();
this->_future.waitForFinished();
}
}
void TaskDispatcher::reset() {
clear();
stop();
}
bool TaskDispatcher::isInterruptionRequested() {
ULock lk1(this->_mutex);
return !_running;
}
bool TaskDispatcher::isRunning() {
ULock lk1(this->_mutex);
return _running;
}
std::size_t TaskDispatcher::pendingTasks() {
ULock lk1(this->_mutex);
return this->_taskQueue.size() + (_running ? 1 : 0);
}
void TaskDispatcher::run() {
while (true) {
ULock lk1(this->_mutex);
if (this->_taskQueue.size() > 0 && this->_running) {
// pop task and promise
auto pTask = std::move(this->_taskQueue.front());
auto promise = std::move(this->_promiseQueue.front());
this->_taskQueue.pop_front();
this->_promiseQueue.pop_front();
lk1.unlock();
// exec task
promise.set_value(pTask->exec());
} else {
this->_running = false;
lk1.unlock();
break;
}
}
}
} // namespace nemo_interface
#ifndef COMMANDDISPATCHER_H
#define COMMANDDISPATCHER_H
#include <QFuture>
#include <QVariant>
#include <deque>
#include "Task.h"
namespace nemo_interface {
class TaskDispatcher : QObject {
public:
TaskDispatcher();
~TaskDispatcher();
//!
//! \brief dispatch Dispatches a task.
//! \param c The task to dispatch.
//! \return Returns a future containing the QVariant which the task c
//! returned.
//!
std::future<QVariant> dispatch(std::unique_ptr<Task> c);
//!
//! \brief dispatchNext Dispatches the task c as the next task.
//! \param c The task to dispatch.
//! \return Returns a future containing the QVariant which the task c
//! returned.
//!
std::future<QVariant> dispatchNext(std::unique_ptr<Task> c);
//!
//! \brief clear Clears the task list.
//!
void clear();
//!
//! \brief start Starts task dispatching. This function can be called after
//! dispatching has been stopped, e.g. by stop() or reset().
//!
void start();
//!
//! \brief stop Stops task dispatching.
//!
//! Stops taks dispatching. The dispatcher will wait for the current task to
//! complete and than terminate the dispatcher thread. After stop() has been
//! called isInterruptionRequested will return true. This can be used to
//! permaturely terminate a task.
//!
void stop();
//!
//! \brief reset Clears the task list and stops dispatching.
//!
void reset();
bool isInterruptionRequested();
bool isRunning();
std::size_t pendingTasks();
protected:
void run();
private:
QFuture<void> _future;
std::deque<std::unique_ptr<Task>> _taskQueue;
std::deque<std::promise<QVariant>> _promiseQueue;
bool _running;
std::mutex _mutex;
};
} // namespace nemo_interface
#endif // COMMANDDISPATCHER_H
......@@ -74,7 +74,8 @@ void RosbridgeWsClient::start(const std::__cxx11::string &client_name,
#ifdef ROS_BRIDGE_DEBUG
std::thread client_thread([client_name, client]() {
#else
std::thread client_thread([client]() {
std::thread client_thread([client_name, client]() {
// std::thread client_thread([client]() {
#endif
client->start();
......
......@@ -41,7 +41,6 @@ bool toJson(const HeartbeatType &heartbeat, rapidjson::Value &value,
template <class HeartbeatType>
bool fromJson(const rapidjson::Value &value, HeartbeatType &heartbeat) {
if (!value.HasMember("status") || !value["status"].IsInt()) {
assert(false);
return false;
}
......
......@@ -24,7 +24,7 @@ const char *idKey = "id";
//! @brief C++ representation of nemo_msgs/labeled_progress message
class LabeledProgress {
public:
LabeledProgress() {}
LabeledProgress() : _id(0), _progress(0) {}
LabeledProgress(double progress, long id) : _id(id), _progress(progress) {}
long id() const { return _id; }
......@@ -52,11 +52,11 @@ bool fromJson(const rapidjson::Value &value, ProgressType &p) {
return false;
}
if (!value.HasMember(idKey) || !value[idKey].IsInt()) {
if (!value.HasMember(idKey) || !value[idKey].IsInt64()) {
return false;
}
p.setId(value[idKey].GetInt());
p.setId(value[idKey].GetInt64());
p.setProgress(value[progressKey].GetDouble());
return true;
......
#include "progress_array.h"
std::string ros_bridge::messages::nemo_msgs::progress_array::messageType() {
return "/nemo_msgs/ProgressArray";
}
#ifndef PROGRESS_ARRAY_H
#define PROGRESS_ARRAY_H
#pragma once
#include "labeled_progress.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include <QVector>
namespace ros_bridge {
//! @brief Namespace containing classes and methodes ros message generation.
namespace messages {
//! @brief Namespace containing classes and methodes for geometry_msgs
//! generation.
namespace nemo_msgs {
//! @brief Namespace containing methodes for nemo_msgs/progress_array message
//! generation.
namespace progress_array {
std::string messageType();
namespace {
const char *progressArrayKey = "progress_array";
} // namespace
//! @brief C++ representation of nemo_msgs/progress_array message
template <template <class, class...> class Container = QVector>
class GenericProgressArray {
public:
typedef Container<labeled_progress::LabeledProgress> ContainerType;
GenericProgressArray() {}
ContainerType &progress_array() { return _progres_array; }
const ContainerType &progress_array() const { return _progres_array; }
void setProgressArray(const ContainerType &c) { _progres_array = c; }
protected:
Container<labeled_progress::LabeledProgress> _progres_array;
};
typedef GenericProgressArray<> ProgressArray;
template <class Array>
bool toJson(const Array &array, rapidjson::Value &value,
rapidjson::Document::AllocatorType &allocator) {
rapidjson::Value jsonArray(rapidjson::kArrayType);
for (const auto &lp : array.progress_array()) {
rapidjson::Value jsonLp;
if (labeled_progress::toJson(lp, jsonLp, allocator)) {
jsonArray.PushBack(jsonLp, allocator);
} else {
return false;
}
}
value.AddMember(rapidjson::Value(progressArrayKey), jsonArray, allocator);
return true;
}
template <class Array> bool fromJson(const rapidjson::Value &value, Array &p) {
if (!value.HasMember(progressArrayKey) ||
!value[progressArrayKey].IsArray()) {
return false;
}
const auto &jsonArray = value[progressArrayKey].GetArray();
p.progress_array().clear();
p.progress_array().reserve(jsonArray.Size());
for (long i = 0; i < jsonArray.Size(); ++i) {
labeled_progress::LabeledProgress lp;
if (!labeled_progress::fromJson(jsonArray[i], lp)) {
return false;
} else {
p.progress_array().push_back(lp);
}
}
return true;
}
} // namespace progress_array
} // namespace nemo_msgs
} // namespace messages
} // namespace ros_bridge
#endif // PROGRESS_ARRAY_H
......@@ -44,9 +44,9 @@ public:
void setTile(const GeoContainer &tile) { _tile = tile; }
protected:
GeoContainer _tile;
long _id;
double _progress;
GeoContainer _tile;
};
typedef GenericTile<> Tile;
......
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