Commit 7308bf55 authored by Valentin Platzgummer's avatar Valentin Platzgummer

nemo interface improved

parent 29514d84
......@@ -463,7 +463,6 @@ HEADERS += \
src/MeasurementComplexItem/nemo_interface/Task.h \
src/MeasurementComplexItem/nemo_interface/TaskDispatcher.h \
src/MeasurementComplexItem/nemo_interface/tileHelper.h \
src/MeasurementComplexItem/routing.h \
src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h \
src/MeasurementComplexItem/nemo_interface/MeasurementTile.h \
src/QmlControls/QmlUnitsConversion.h \
......@@ -539,7 +538,6 @@ SOURCES += \
src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp \
src/MeasurementComplexItem/nemo_interface/Task.cpp \
src/MeasurementComplexItem/nemo_interface/TaskDispatcher.cpp \
src/MeasurementComplexItem/routing.cpp \
src/Vehicle/VehicleEscStatusFactGroup.cc \
src/MeasurementComplexItem/AreaData.cc \
src/api/QGCCorePlugin.cc \
......
......@@ -7,7 +7,7 @@
#include "SettingsManager.h"
#include "WimaSettings.h"
#include <shared_mutex>
#include <mutex>
#include <QJsonArray>
#include <QJsonObject>
......@@ -17,7 +17,6 @@
#include "GenericSingelton.h"
#include "geometry/MeasurementArea.h"
#include "geometry/geometry.h"
#include "nemo_interface/FutureWatcher.h"
#include "nemo_interface/MeasurementTile.h"
#include "nemo_interface/QNemoHeartbeat.h"
#include "nemo_interface/TaskDispatcher.h"
......@@ -29,6 +28,30 @@
#include "ros_bridge/include/messages/nemo_msgs/tile_array.h"
#include "rosbridge/rosbridge.h"
/*
* Here some rules:
* * not threas safe functions are marked with *NotSafe(...)
* * If a function is not safe:
* * the call to it must be protected with a Lock.
* * not safe functions are not allowed to emit signals directly (danger of
* deadlock).
* * if a not safe function needs to emit a signal, defere it with
* QTimer::singleShot().
* * not safe functions are allowed to call other not safe functions
* * it is a bad idea to wait inside a not safe function for a asynchronous
* operation (potential deadlock)
* * Functions that are not marked with *NotSafe(...) must be thread safe!
*/
#define INVM(context, fun) \
{ \
auto value = QMetaObject::invokeMethod(context, fun); \
Q_ASSERT(value == true); \
Q_UNUSED(value); \
}
Q_DECLARE_METATYPE(ProgressArray)
QGC_LOGGING_CATEGORY(NemoInterfaceLog, "NemoInterfaceLog")
#define NO_HEARTBEAT_TIMEOUT 5000 // ms
......@@ -36,10 +59,32 @@ QGC_LOGGING_CATEGORY(NemoInterfaceLog, "NemoInterfaceLog")
#define RESTART_RETRY_INTERVAl 2000 // ms
#define SYNC_INTERVAL 10000 // ms
#define SYNC_RETRY_INTERVAL 2000 // ms
static constexpr auto maxResponseTime = std::chrono::milliseconds(10000);
static const char *progressTopic = "/nemo/progress";
static const char *heartbeatTopic = "/nemo/heartbeat";
static char const *progressTopic = "/nemo/progress";
static char const *heartbeatTopic = "/nemo/heartbeat";
static char const *addTilesService = "/nemo/add_tiles";
static char const *removeTilesService = "/nemo/remove_tiles";
static char const *clearTilesService = "/nemo/clear_tiles";
static char const *getAllTilesService = "/nemo/get_all_tiles";
// static char const *getTilesService = "/nemo/get_tiles";
// static char const *containsTilesService = "/nemo/contains_tiles";
// static char const *extractTilesService = "/nemo/extract_tiles";
// static char const *sizeService = "/nemo/size";
// static char const *emptyService = "/nemo/empty";
static char const *getProgressService = "/nemo/get_progress";
static char const *getAllProgressService = "/nemo/get_all_progress";
static char const *getVersionService = "/nemo/get_version";
static const std::vector<char const *> requiredServices{
addTilesService, removeTilesService, clearTilesService,
getAllTilesService, getAllProgressService, getProgressService,
getVersionService};
static const std::vector<char const *> requiredTopics{progressTopic,
heartbeatTopic};
using hrc = std::chrono::high_resolution_clock;
using ROSBridgePtr = std::shared_ptr<Rosbridge>;
......@@ -51,8 +96,7 @@ typedef std::map<QString, std::shared_ptr<Tile>> TileMap;
typedef std::map<QString, std::shared_ptr<const Tile>> TileMapConst;
typedef ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat Heartbeat;
typedef nemo_interface::TaskDispatcher Dispatcher;
typedef nemo_interface::FutureWatcher<QVariant, std::shared_future>
FutureWatcher;
typedef std::unique_lock<std::mutex> Lock;
class NemoInterface::Impl {
enum class STATE {
......@@ -62,6 +106,7 @@ class NemoInterface::Impl {
TRY_SETUP,
USER_SYNC,
SYS_SYNC,
SYNC_ERROR,
READY,
WEBSOCKET_TIMEOUT,
HEARTBEAT_TIMEOUT
......@@ -99,96 +144,84 @@ public:
const QString &warningString() const;
private:
void _checkVersion();
void _subscribeProgressTopic();
void _subscribeHearbeatTopic();
void _doAction();
void _trySynchronize();
void _synchronizeIfNeccessary();
void _doSetup();
void _doActionNotSafe();
void _synchronize();
void _tryRestart();
bool _isSynchronizedNotSafe() const;
bool _isSynchronized() const;
bool _userSync() const; // thread safe
bool _sysSync() const; // thread safe
void _onFutureWatcherFinished(); // thread safe
void _onHeartbeatTimeout(); // thread safe
void _onHeartbeatTimeout();
void _onRosbridgeStateChanged();
// called from dispatcher thread!
QVariant _callAddTiles(
std::shared_ptr<QVector<std::shared_ptr<const Tile>>> pTileArray);
// called from dispatcher thread!
QVariant _callRemoveTiles(std::shared_ptr<IDArray> pIdArray);
// called from dispatcher thread!
QVariant _callClearTiles();
// called from dispatcher thread!
QVariant _callGetProgress(std::shared_ptr<IDArray> pIdArray);
QVariant _callGetAllProgress();
QVariant _callGetAllTiles();
QVariant _callGetVersion();
enum class CALL_NAME {
ADD_TILES,
REMOVE_TILES,
CLEAR_TILES,
GET_PROGRESS,
GET_ALL_TILES,
GET_ALL_PROGRESS,
GET_VERSION
};
QString _toString(CALL_NAME name);
void _addTilesRemote(
std::shared_ptr<QVector<std::shared_ptr<const Tile>>> pTileArray,
std::promise<bool> promise);
void
_addTilesRemote2(std::shared_ptr<QVector<std::shared_ptr<Tile>>> pTileArray,
std::promise<bool> promise);
void
_compareAndSync(std::shared_ptr<QVector<std::shared_ptr<Tile>>> pTileArray,
std::promise<bool> promise);
void _setVersion(QString version, std::promise<bool> promise);
void _removeTilesRemote(std::shared_ptr<IDArray> idArray,
std::promise<bool> promise);
void _clearTilesRemote(std::promise<bool> promise);
void _updateProgress(std::shared_ptr<ProgressArray> pArray,
std::promise<bool> promise);
void _onHeartbeatReceived(const QNemoHeartbeat &hb,
std::promise<bool> promise);
bool _updateProgress(const ProgressArray &pArray);
void _onHeartbeatReceived(const QNemoHeartbeat &hb);
void _setInfoStringNotSafe(const QString &info);
void _setWarningStringNotSafe(const QString &warning);
void _setInfoString(const QString &info);
void _setWarningString(const QString &warning);
bool _setState(STATE newState); // not thread safe
static bool _ready(STATE s);
static bool _userSync(STATE s);
static bool _sysSync(STATE s);
static bool _running(STATE s);
// state suff
bool _userSync() const;
bool _sysSync() const;
bool _setStateNotSafe(STATE newState);
static bool _readyNotSafe(STATE s);
static bool _userSyncNotSafe(STATE s);
static bool _sysSyncNotSafe(STATE s);
static bool _runningNotSafe(STATE s);
static NemoInterface::STATUS _status(STATE state);
static QString _toString(STATE s);
static QString _toString(NemoInterface::STATUS s);
static QString _localVersion;
QString _remoteVersion;
// impl functions
bool _addTilesImpl(
std::shared_ptr<QVector<std::shared_ptr<const Tile>>> pTileArray,
std::shared_ptr<const IDArray> pIdArray);
bool _removeTilesImpl(std::shared_ptr<const IDArray> pIdArray);
bool _clearTilesImpl();
// Worker functions.
static bool
_callAddTiles(const QVector<std::shared_ptr<const Tile>> &tileArray,
Dispatcher &d, Rosbridge &rb);
static bool _callClearTiles(Dispatcher &d, Rosbridge &rb);
static ProgressArray _callGetProgress(const IDArray &pIdArray, Dispatcher &d,
Rosbridge &rb);
static ProgressArray _callGetAllProgress(Dispatcher &d, Rosbridge &rb);
static QVector<std::shared_ptr<Tile>> _callGetAllTiles(Dispatcher &d,
Rosbridge &rb);
static QString _callGetVersion(Dispatcher &d, Rosbridge &rb);
static bool _callRemoveTiles(const IDArray &pIdArray, Dispatcher &d,
Rosbridge &rb);
// functions to manipulate _remoteTiles
bool _addTilesRemote(const QVector<std::shared_ptr<const Tile>> tileArray);
bool _addTilesRemote(const QVector<std::shared_ptr<Tile>> &tileArray);
void _removeTilesRemote(const IDArray &idArray);
void _clearTilesRemote();
void _clearTilesRemoteNotSafe();
// static and const members
static const char *_localVersion;
NemoInterface *const _parent;
std::atomic<STATE> _state;
std::atomic_bool _versionOK;
std::atomic_bool _progressTopicOK;
std::atomic_bool _heartbeatTopicOK;
std::atomic<CALL_NAME> _lastCall;
// thread safe members
ROSBridgePtr _pRosbridge;
Dispatcher _dispatcher;
// protected by mutex
mutable std::mutex _m;
STATE _state;
TileMap _remoteTiles;
TileMapConst _localTiles;
NemoInterface *const _parent;
Dispatcher _dispatcher;
QString _infoString;
QString _warningString;
// Members belonging to _parent->thread()
QTimer _timeoutTimer;
QTimer _syncTimer;
QTimer _restartTimer;
QNemoHeartbeat _lastHeartbeat;
FutureWatcher _futureWatcher;
};
QString NemoInterface::Impl::_localVersion("V_1.0");
const char *NemoInterface::Impl::_localVersion("V_1.0");
using StatusMap = std::map<NemoInterface::STATUS, QString>;
static StatusMap statusMap{
......@@ -206,8 +239,7 @@ static StatusMap statusMap{
NemoInterface::STATUS::WEBSOCKET_DETECTED, "Websocket Detected")};
NemoInterface::Impl::Impl(NemoInterface *p)
: _state(STATE::STOPPED), _versionOK(false), _progressTopicOK(false),
_heartbeatTopicOK(false), _parent(p) {
: _parent(p), _state(STATE::STOPPED) {
// ROS Bridge.
WimaSettings *wimaSettings =
......@@ -224,6 +256,7 @@ NemoInterface::Impl::Impl(NemoInterface *p)
this->start();
}
};
connect(connectionStringFact, &SettingsFact::rawValueChanged,
setConnectionString);
setConnectionString();
......@@ -235,29 +268,31 @@ NemoInterface::Impl::Impl(NemoInterface *p)
connect(this->_pRosbridge.get(), &Rosbridge::stateChanged, this->_parent,
[this] { this->_onRosbridgeStateChanged(); });
connect(&this->_futureWatcher, &FutureWatcher::finished, this->_parent,
[this] { this->_onFutureWatcherFinished(); });
connect(&this->_restartTimer, &QTimer::timeout, this->_parent,
[this] { this->_tryRestart(); });
connect(&this->_syncTimer, &QTimer::timeout, this->_parent,
[this] { this->_synchronizeIfNeccessary(); });
[this] { this->_synchronize(); });
static std::once_flag flag;
std::call_once(flag, [] { qRegisterMetaType<ProgressArray>(); });
}
NemoInterface::Impl::~Impl() { this->_pRosbridge->stop(); }
void NemoInterface::Impl::start() {
if (!running()) {
this->_setState(STATE::START_BRIDGE);
this->_doAction();
Lock lk(this->_m);
if (!_runningNotSafe(this->_state)) {
this->_setStateNotSafe(STATE::START_BRIDGE);
this->_doActionNotSafe();
}
}
void NemoInterface::Impl::stop() {
if (running()) {
this->_setState(STATE::STOPPED);
this->_doAction();
Lock lk(this->_m);
if (_runningNotSafe(this->_state)) {
this->_setStateNotSafe(STATE::STOPPED);
this->_doActionNotSafe();
}
}
......@@ -272,6 +307,8 @@ NemoInterface::Impl::addTiles(const TilePtrArray &tileArray) {
// copy unknown tiles
auto pTileArray = std::make_shared<QVector<std::shared_ptr<const Tile>>>();
auto pIdArray = std::make_shared<IDArray>();
Lock lk(this->_m);
for (const auto *pTile : tileArray) {
auto id = pTile->id();
const auto it = this->_localTiles.find(id);
......@@ -288,38 +325,36 @@ NemoInterface::Impl::addTiles(const TilePtrArray &tileArray) {
}
}
if (pTileArray->size() > 0) {
lk.unlock();
emit this->_parent->tilesChanged();
lk.lock();
}
// ready for send?
if (pTileArray->size() > 0 && (this->ready() || this->_userSync())) {
this->_setState(STATE::USER_SYNC);
this->_doAction();
// create add tiles command.
auto pTask = std::make_unique<Task>(
std::bind(&Impl::_callAddTiles, this, pTileArray));
// dispatch command.
auto ret = _dispatcher.dispatch(std::move(pTask));
auto addFuture = ret.share();
// create get progress cmd.
pTask = std::make_unique<Task>([this, addFuture, pIdArray] {
addFuture.wait();
if (addFuture.get().toBool()) {
return this->_callGetProgress(pIdArray);
} else {
return QVariant(false);
if (pTileArray->size() > 0 && (this->_readyNotSafe(this->_state) ||
this->_userSyncNotSafe(this->_state))) {
this->_setStateNotSafe(STATE::USER_SYNC);
this->_doActionNotSafe();
lk.unlock();
// create task.
auto pTask = std::make_unique<Task>([this, pTileArray, pIdArray] {
auto ret = this->_addTilesImpl(pTileArray, pIdArray);
if (ret) {
Lock lk(this->_m);
if (this->_isSynchronizedNotSafe()) {
this->_setStateNotSafe(STATE::READY);
this->_doActionNotSafe();
}
}
return ret;
});
// dispatch command.
ret = _dispatcher.dispatch(std::move(pTask));
auto progressFuture = ret.share();
_futureWatcher.setFuture(progressFuture);
return progressFuture;
auto ret = _dispatcher.dispatch(std::move(pTask));
return ret.share();
}
}
......@@ -338,6 +373,8 @@ NemoInterface::Impl::removeTiles(const IDArray &idArray) {
// copy known ids
auto pIdArray = std::make_shared<IDArray>();
Lock lk(this->_m);
for (const auto &id : idArray) {
const auto it = this->_localTiles.find(id);
Q_ASSERT(it != _localTiles.end());
......@@ -349,23 +386,37 @@ NemoInterface::Impl::removeTiles(const IDArray &idArray) {
}
}
if (pIdArray->size() > 0) {
lk.unlock();
emit this->_parent->tilesChanged();
lk.lock();
}
// ready for send?
if (pIdArray->size() > 0 && (this->ready() || this->_userSync())) {
if (pIdArray->size() > 0 && (this->_readyNotSafe(this->_state) ||
this->_userSyncNotSafe(this->_state))) {
this->_setState(STATE::USER_SYNC);
this->_doAction();
this->_setStateNotSafe(STATE::USER_SYNC);
this->_doActionNotSafe();
lk.unlock();
// create command.
auto cmd = std::make_unique<Task>(
std::bind(&Impl::_callRemoveTiles, this, pIdArray));
auto cmd = std::make_unique<Task>([this, pIdArray] {
auto ret = this->_removeTilesImpl(pIdArray);
if (ret) {
Lock lk(this->_m);
if (this->_isSynchronizedNotSafe()) {
this->_setStateNotSafe(STATE::READY);
this->_doActionNotSafe();
}
}
return ret;
});
// dispatch command and return.
auto ret = _dispatcher.dispatch(std::move(cmd));
auto sfut = ret.share();
_futureWatcher.setFuture(sfut);
return sfut;
}
}
......@@ -381,26 +432,40 @@ std::shared_future<QVariant> NemoInterface::Impl::clearTiles() {
// qDebug() << "clearTiles called";
// clear local tiles (_localTiles)
Lock lk(this->_m);
if (!_localTiles.empty()) {
this->_localTiles.clear();
lk.unlock();
emit this->_parent->tilesChanged();
lk.lock();
}
if (this->ready() || this->_userSync()) {
if (this->_readyNotSafe(this->_state) || this->_readyNotSafe(this->_state)) {
this->_setState(STATE::USER_SYNC);
this->_doAction();
this->_setStateNotSafe(STATE::USER_SYNC);
this->_doActionNotSafe();
lk.unlock();
// create command.
auto pTask =
std::make_unique<Task>(std::bind(&Impl::_callClearTiles, this));
auto pTask = std::make_unique<Task>([this] {
auto ret = this->_clearTilesImpl();
if (ret) {
Lock lk(this->_m);
if (this->_isSynchronizedNotSafe()) {
this->_setStateNotSafe(STATE::READY);
this->_doActionNotSafe();
}
}
return QVariant(ret);
});
// dispatch command and return.
auto ret = _dispatcher.dispatch(std::move(pTask));
auto sfut = ret.share();
_futureWatcher.setFuture(sfut);
return sfut;
return ret.share();
} else {
lk.unlock();
std::promise<QVariant> p;
p.set_value(QVariant(false));
return p.get_future();
......@@ -410,6 +475,7 @@ std::shared_future<QVariant> NemoInterface::Impl::clearTiles() {
TileArray NemoInterface::Impl::getTiles(const IDArray &idArray) const {
TileArray tileArray;
Lock lk(this->_m);
if (this->ready()) {
for (const auto &id : idArray) {
const auto it = _remoteTiles.find(id);
......@@ -440,6 +506,7 @@ TileArray NemoInterface::Impl::getTiles(const IDArray &idArray) const {
TileArray NemoInterface::Impl::getAllTiles() const {
TileArray tileArray;
Lock lk(this->_m);
if (this->ready()) {
for (const auto &entry : _remoteTiles) {
auto pTile = entry.second;
......@@ -467,6 +534,7 @@ TileArray NemoInterface::Impl::getAllTiles() const {
LogicalArray NemoInterface::Impl::containsTiles(const IDArray &idArray) const {
LogicalArray logicalArray;
Lock lk(this->_m);
for (const auto &id : idArray) {
const auto &it = _localTiles.find(id);
logicalArray.append(it != _localTiles.end());
......@@ -475,14 +543,22 @@ LogicalArray NemoInterface::Impl::containsTiles(const IDArray &idArray) const {
return logicalArray;
}
std::size_t NemoInterface::Impl::size() const { return _localTiles.size(); }
std::size_t NemoInterface::Impl::size() const {
Lock lk(this->_m);
return _localTiles.size();
}
bool NemoInterface::Impl::empty() const { return _localTiles.empty(); }
bool NemoInterface::Impl::empty() const {
Lock lk(this->_m);
return _localTiles.empty();
}
ProgressArray NemoInterface::Impl::getProgress() const {
ProgressArray progressArray;
if (this->_isSynchronized()) {
Lock lk(this->_m);
if (this->_isSynchronizedNotSafe()) {
for (const auto &entry : _remoteTiles) {
progressArray.append(
LabeledProgress{entry.second->progress(), entry.second->id()});
......@@ -500,7 +576,8 @@ ProgressArray NemoInterface::Impl::getProgress() const {
ProgressArray NemoInterface::Impl::getProgress(const IDArray &idArray) const {
ProgressArray progressArray;
if (this->_isSynchronized()) {
Lock lk(this->_m);
if (this->_isSynchronizedNotSafe()) {
for (const auto &id : idArray) {
const auto it = _remoteTiles.find(id);
if (it != _remoteTiles.end()) {
......@@ -522,173 +599,178 @@ ProgressArray NemoInterface::Impl::getProgress(const IDArray &idArray) const {
}
NemoInterface::STATUS NemoInterface::Impl::status() const {
Lock lk(this->_m);
return _status(this->_state);
}
bool NemoInterface::Impl::running() const { return _running(this->_state); }
bool NemoInterface::Impl::ready() const { return _ready(this->_state.load()); }
bool NemoInterface::Impl::_sysSync() const { return _sysSync(this->_state); }
bool NemoInterface::Impl::running() const {
Lock lk1(this->_m);
return _runningNotSafe(this->_state);
}
void NemoInterface::Impl::_onFutureWatcherFinished() {
if (this->ready() || this->_userSync() || this->_sysSync()) {
static long tries = 0;
auto lastTransactionSuccessfull = _futureWatcher.result().toBool();
if (!lastTransactionSuccessfull) {
++tries;
qCDebug(NemoInterfaceLog)
<< "last transaction unsuccessfull: " << _toString(_lastCall);
bool NemoInterface::Impl::ready() const {
Lock lk1(this->_m);
return _readyNotSafe(this->_state);
}
if (tries < 5) {
QTimer::singleShot(SYNC_RETRY_INTERVAL, this->_parent,
[this] { this->_trySynchronize(); });
} else {
_setWarningString("The last 5 transactions failed! Please check the "
"connection and consider reseting the connection.");
tries = 0;
}
} else {
tries = 0;
}
}
bool NemoInterface::Impl::_sysSync() const {
Lock lk1(this->_m);
return _sysSyncNotSafe(this->_state);
}
void NemoInterface::Impl::_onHeartbeatTimeout() {
this->_setState(STATE::HEARTBEAT_TIMEOUT);
this->_doAction();
Lock lk1(this->_m);
this->_setStateNotSafe(STATE::HEARTBEAT_TIMEOUT);
this->_doActionNotSafe();
}
void NemoInterface::Impl::_onRosbridgeStateChanged() {
auto state = this->_pRosbridge->state();
if (state == Rosbridge::STATE::CONNECTED) {
auto rbState = this->_pRosbridge->state();
if (rbState == Rosbridge::STATE::CONNECTED) {
Lock lk1(this->_m);
if (this->_state == STATE::START_BRIDGE ||
this->_state == STATE::WEBSOCKET_TIMEOUT) {
this->_setState(STATE::WEBSOCKET_DETECTED);
this->_doAction();
this->_setStateNotSafe(STATE::WEBSOCKET_DETECTED);
this->_doActionNotSafe();
}
} else if (state == Rosbridge::STATE::TIMEOUT) {
} else if (rbState == Rosbridge::STATE::TIMEOUT) {
Lock lk1(this->_m);
if (this->_state == STATE::TRY_SETUP || this->_state == STATE::READY ||
this->_state == STATE::WEBSOCKET_DETECTED ||
this->_state == STATE::HEARTBEAT_TIMEOUT) {
this->_setState(STATE::WEBSOCKET_TIMEOUT);
this->_doAction();
this->_setStateNotSafe(STATE::WEBSOCKET_TIMEOUT);
this->_doActionNotSafe();
}
}
}
bool NemoInterface::Impl::_userSync() const { return _userSync(this->_state); }
bool NemoInterface::Impl::_userSync() const {
Lock lk1(this->_m);
return _userSyncNotSafe(this->_state);
}
const QString &NemoInterface::Impl::infoString() const { return _infoString; }
const QString &NemoInterface::Impl::infoString() const {
Lock lk1(this->_m);
return _infoString;
}
const QString &NemoInterface::Impl::warningString() const {
Lock lk1(this->_m);
return _warningString;
}
void NemoInterface::Impl::_updateProgress(std::shared_ptr<ProgressArray> pArray,
std::promise<bool> promise) {
// qDebug() << "_updateProgress called";
bool NemoInterface::Impl::_updateProgress(const ProgressArray &pArray) {
ProgressArray copy;
bool error = false;
for (auto itLP = pArray->begin(); itLP != pArray->end();) {
Lock lk1(this->_m);
for (auto itLP = pArray.begin(); itLP != pArray.end(); ++itLP) {
auto it = _remoteTiles.find(itLP->id());
if (Q_LIKELY(it != _remoteTiles.end())) {
it->second->setProgress(itLP->progress());
++itLP;
copy.push_back(*itLP);
} else {
qCDebug(NemoInterfaceLog)
<< "_updateProgress(): tile with id " << itLP->id() << " not found.";
itLP = pArray->erase(itLP);
error = true;
}
}
lk1.unlock();
if (pArray->size() > 0) {
emit _parent->progressChanged(*pArray);
if (copy.size() > 0) {
emit _parent->progressChanged(copy);
}
promise.set_value(!error);
return !error;
}
void NemoInterface::Impl::_onHeartbeatReceived(const QNemoHeartbeat &hb,
std::promise<bool> promise) {
_lastHeartbeat = hb;
this->_timeoutTimer.start(NO_HEARTBEAT_TIMEOUT);
if (this->_state == STATE::TRY_SETUP) {
this->_setState(STATE::READY);
this->_doAction();
} else if (this->_state == STATE::HEARTBEAT_TIMEOUT) {
this->_setState(STATE::READY);
this->_doAction();
void NemoInterface::Impl::_onHeartbeatReceived(const QNemoHeartbeat &) {
INVM(this->_parent,
([this]() mutable { this->_timeoutTimer.start(NO_HEARTBEAT_TIMEOUT); }))
Lock lk(this->_m);
if (this->_state == STATE::TRY_SETUP ||
this->_state == STATE::HEARTBEAT_TIMEOUT) {
this->_setStateNotSafe(STATE::READY);
this->_doActionNotSafe();
}
promise.set_value(true);
}
void NemoInterface::Impl::_setInfoString(const QString &info) {
void NemoInterface::Impl::_setInfoStringNotSafe(const QString &info) {
if (_infoString != info) {
_infoString = info;
emit this->_parent->infoStringChanged();
// emit signal later, can't emit while mutex locked!
QTimer::singleShot(5, this->_parent,
[this] { emit this->_parent->infoStringChanged(); });
}
}
void NemoInterface::Impl::_setWarningString(const QString &warning) {
void NemoInterface::Impl::_setWarningStringNotSafe(const QString &warning) {
if (_warningString != warning) {
_warningString = warning;
emit this->_parent->warningStringChanged();
// emit signal later, can't emit while mutex locked!
QTimer::singleShot(5, this->_parent,
[this] { emit this->_parent->warningStringChanged(); });
}
}
void NemoInterface::Impl::_checkVersion() {
void NemoInterface::Impl::_setInfoString(const QString &info) {
Lock lk(this->_m);
_setInfoStringNotSafe(info);
}
void NemoInterface::Impl::_setWarningString(const QString &warning) {
Lock lk(this->_m);
_setWarningStringNotSafe(warning);
}
void NemoInterface::Impl::_doSetup() {
// create task
auto pTask = std::make_unique<nemo_interface::Task>([this] {
// wait for service
std::future<bool> fut;
do {
fut = this->_pRosbridge->serviceAvailable("/nemo/get_version");
// wait
while (true) {
auto status = fut.wait_for(std::chrono::milliseconds(5));
if (this->_dispatcher.isInterruptionRequested()) {
return QVariant(false);
}
if (status == std::future_status::ready) {
break;
}
}
} while (!fut.get());
auto rb = this->_pRosbridge;
auto &disp = this->_dispatcher;
// call service
return this->_callGetVersion();
});
// check if required services are available
auto cond = [&disp] { return !disp.isInterruptionRequested(); };
for (const auto &cc : requiredServices) {
QString s(cc);
this->_dispatcher.dispatch(std::move(pTask));
}
this->_setInfoString("Waiting for required service " + s);
void NemoInterface::Impl::_subscribeProgressTopic() {
auto pTask = std::make_unique<nemo_interface::Task>([this] {
// wait for service
std::future<bool> fut;
do {
fut = this->_pRosbridge->topicAvailable(progressTopic);
// wait
while (true) {
auto status = fut.wait_for(std::chrono::milliseconds(5));
if (this->_dispatcher.isInterruptionRequested()) {
return QVariant(false);
}
if (status == std::future_status::ready) {
break;
}
auto available = rb->waitForService(cc, cond);
if (!available) {
return QVariant(false);
}
} while (!fut.get());
}
// subscribe
this->_pRosbridge->subscribeTopic(progressTopic, [this](
const QJsonObject &o) {
// check if version is compatible{
this->_setInfoString("Checking version.");
auto version = _callGetVersion(disp, *rb);
if (version != _localVersion) {
this->_setWarningString(
"Version checking failed. Local protocol version (" +
QString(_localVersion) + ") does not match remote version (" +
version + ").");
return QVariant(false);
}
// check if required topics are available
for (const auto &cc : requiredTopics) {
QString s(cc);
this->_setInfoString("Waiting for required topic " + s);
auto available = rb->waitForTopic(cc, cond);
if (!available) {
return QVariant(false);
}
}
// subscribe topics
rb->subscribeTopic(progressTopic, [this](const QJsonObject &o) {
ros_bridge::messages::nemo_msgs::progress_array::ProgressArray
progressArray;
if (ros_bridge::messages::nemo_msgs::progress_array::fromJson(
......@@ -707,167 +789,161 @@ void NemoInterface::Impl::_subscribeProgressTopic() {
}
if (rangeError) {
qCWarning(NemoInterfaceLog) << "/nemo/progress progress out "
qCWarning(NemoInterfaceLog) << progressTopic
<< " progress out "
"of range, value was set to: "
<< lp.progress();
}
}
auto p = std::make_shared<ProgressArray>();
*p = std::move(progressArray.progress_array());
std::promise<bool> promise;
auto future = promise.get_future();
bool value = QMetaObject::invokeMethod(
this->_parent, [this, p, promise = std::move(promise)]() mutable {
this->_updateProgress(p, std::move(promise));
});
Q_ASSERT(value == true);
future.wait();
auto ret = this->_updateProgress(progressArray.progress_array());
if (!ret)
this->_setWarningString("Progress update failed.");
} else {
qCWarning(NemoInterfaceLog) << "/nemo/progress not able to "
qCWarning(NemoInterfaceLog) << progressTopic
<< " not able to "
"create ProgressArray form json: "
<< o;
}
});
this->_progressTopicOK = true;
bool value = QMetaObject::invokeMethod(this->_parent /* context */,
[this]() { this->_doAction(); });
Q_ASSERT(value == true);
return QVariant(true);
});
this->_dispatcher.dispatch(std::move(pTask));
}
void NemoInterface::Impl::_subscribeHearbeatTopic() {
auto pTask = std::make_unique<nemo_interface::Task>([this] {
// wait for service
std::future<bool> fut;
do {
fut = this->_pRosbridge->topicAvailable(heartbeatTopic);
// wait
while (true) {
auto status = fut.wait_for(std::chrono::milliseconds(5));
if (this->_dispatcher.isInterruptionRequested()) {
return QVariant(false);
}
if (status == std::future_status::ready) {
break;
}
}
} while (!fut.get());
// subscribe
using namespace ros_bridge::messages;
this->_pRosbridge->subscribeTopic(
heartbeatTopic, [this](const QJsonObject &o) {
nemo_msgs::heartbeat::Heartbeat heartbeat;
if (nemo_msgs::heartbeat::fromJson(o, heartbeat)) {
std::promise<bool> promise;
auto future = promise.get_future();
bool value = QMetaObject::invokeMethod(
this->_parent,
[this, heartbeat, promise = std::move(promise)]() mutable {
this->_onHeartbeatReceived(heartbeat, std::move(promise));
});
Q_ASSERT(value == true);
future.wait();
} else {
qCWarning(NemoInterfaceLog) << "/nemo/heartbeat not able to "
"create Heartbeat form json: "
<< o;
}
});
this->_heartbeatTopicOK = true;
bool value = QMetaObject::invokeMethod(this->_parent /* context */,
[this]() { this->_doAction(); });
Q_ASSERT(value == true);
rb->subscribeTopic(heartbeatTopic, [this](const QJsonObject &o) {
nemo_msgs::heartbeat::Heartbeat heartbeat;
if (nemo_msgs::heartbeat::fromJson(o, heartbeat)) {
this->_onHeartbeatReceived(heartbeat);
} else {
qCWarning(NemoInterfaceLog) << heartbeatTopic
<< " not able to "
"create Heartbeat form json: "
<< o;
}
});
// now ready
INVM(this->_parent, ([this]() mutable {
this->_timeoutTimer.start(NO_HEARTBEAT_TIMEOUT);
}))
_setInfoString("");
_setWarningString("");
return QVariant(true);
});
this->_dispatcher.dispatch(std::move(pTask));
auto f = _dispatcher.dispatch(std::move(pTask));
Q_UNUSED(f);
}
void NemoInterface::Impl::_trySynchronize() {
if ((this->_state == STATE::READY || this->_state == STATE::SYS_SYNC ||
this->_state == STATE::USER_SYNC) &&
!_isSynchronized()) {
void NemoInterface::Impl::_synchronize() {
Lock lk(this->_m);
if (this->_state == STATE::READY || this->_state == STATE::SYNC_ERROR) {
if (!_dispatcher.idle()) {
QTimer::singleShot(SYNC_RETRY_INTERVAL, this->_parent,
[this] { this->_trySynchronize(); });
qCWarning(NemoInterfaceLog) << "synchronization defered";
return;
}
_setStateNotSafe(STATE::SYS_SYNC);
_doActionNotSafe();
qCWarning(NemoInterfaceLog) << "trying to synchronize";
// copy tiles.
auto pTileArray = std::make_shared<QVector<std::shared_ptr<const Tile>>>();
for (auto it = _localTiles.begin(); it != _localTiles.end(); ++it) {
pTileArray->push_back(it->second);
}
lk.unlock();
// create cmd.
auto pTask = std::make_unique<nemo_interface::Task>([this, pTileArray] {
auto rb = this->_pRosbridge;
auto &disp = this->_dispatcher;
// are _localTiles and _remoteTiles synced?
auto remoteTiles = this->_callGetAllTiles(disp, *rb);
// create tile map;
std::map<QString, std::shared_ptr<Tile>> tempMap;
for (auto &&pTile : remoteTiles) {
auto it = tempMap.find(pTile->id());
if (Q_LIKELY(it == tempMap.end())) {
tempMap.insert(std::make_pair(pTile->id(), pTile));
} else {
qCDebug(NemoInterfaceLog)
<< "_synchronizeIfNeccessary(): remoteTiles contains "
"duplicate id";
}
}
this->_setState(STATE::SYS_SYNC);
this->_doAction();
// compare with pTileArray
bool synced = true;
for (auto &&pTile : *pTileArray) {
auto it = tempMap.find(pTile->id());
if (it == tempMap.end() || it->second->tile() != pTile->tile()) {
synced = false;
break;
}
}
// create clear cmd.
auto pTask = std::make_unique<nemo_interface::Task>(
std::bind(&Impl::_callClearTiles, this));
if (!synced) {
auto success = this->_clearTilesImpl();
if (!success) {
Lock lk(this->_m);
_setStateNotSafe(STATE::SYNC_ERROR);
_doActionNotSafe();
return QVariant(false);
}
// dispatch command.
Q_ASSERT(_dispatcher.pendingTasks() == 0);
auto ret = _dispatcher.dispatch(std::move(pTask));
auto clearFuture = ret.share();
auto pIdArray = std::make_shared<IDArray>();
for (auto &&pTile : *pTileArray) {
pIdArray->push_back(pTile->id());
}
// create tile array.
auto pTileArray = std::make_shared<QVector<std::shared_ptr<const Tile>>>();
for (const auto &pair : _localTiles) {
pTileArray->push_back(pair.second);
}
success = this->_addTilesImpl(pTileArray, pIdArray);
// create addTiles cmd.
pTask =
std::make_unique<nemo_interface::Task>([this, pTileArray, clearFuture] {
clearFuture.wait();
if (clearFuture.get().toBool()) {
return this->_callAddTiles(pTileArray);
} else {
return QVariant(false);
if (!success) {
Lock lk(this->_m);
_setStateNotSafe(STATE::SYNC_ERROR);
_doActionNotSafe();
return QVariant(false);
}
} else {
// update progress
this->_clearTilesRemote();
auto ret = this->_addTilesRemote(*pTileArray);
if (ret) {
ProgressArray progress;
for (auto &&pTile : remoteTiles) {
progress.push_back(LabeledProgress(pTile->progress(), pTile->id()));
}
});
ret = _updateProgress(progress);
}
// dispatch command.
ret = _dispatcher.dispatch(std::move(pTask));
auto addFuture = ret.share();
// create GetAllProgress cmd.
pTask = std::make_unique<nemo_interface::Task>([this, addFuture] {
addFuture.wait();
if (addFuture.get().toBool()) {
return this->_callGetAllProgress();
} else {
return QVariant(false);
if (!ret) {
Lock lk(this->_m);
this->_setStateNotSafe(STATE::SYNC_ERROR);
this->_doActionNotSafe();
lk.unlock();
this->_setWarningString("Getting progress failed.");
qCDebug(NemoInterfaceLog)
<< "_addTilesImpl(): _updateProgress failed: unknown id.";
return QVariant(false);
}
}
});
// dispatch command.
ret = _dispatcher.dispatch(std::move(pTask));
_futureWatcher.setFuture(ret.share());
}
}
// check if local versions are still synced (user could habe modified
// _localTiles).
Lock lk(this->_m);
if (_isSynchronizedNotSafe()) {
_setStateNotSafe(STATE::READY);
_doActionNotSafe();
} else {
INVM(this->_parent, [this] { this->_synchronize(); })
}
lk.unlock();
void NemoInterface::Impl::_synchronizeIfNeccessary() {
if (_dispatcher.idle()) {
// create getAllTiles cmd.
auto pTask = std::make_unique<nemo_interface::Task>(
[this] { return this->_callGetAllTiles(); });
return QVariant(true);
});
// dispatch command.
auto ret = _dispatcher.dispatch(std::move(pTask));
auto fut = ret.share();
_futureWatcher.setFuture(fut);
_syncTimer.start(SYNC_INTERVAL);
Q_UNUSED(ret);
INVM(this->_parent, [this] { this->_syncTimer.start(SYNC_INTERVAL); })
} else {
_syncTimer.start(SYNC_RETRY_INTERVAL);
INVM(this->_parent, [this] { this->_syncTimer.start(SYNC_RETRY_INTERVAL); })
}
}
......@@ -884,88 +960,103 @@ void NemoInterface::Impl::_tryRestart() {
}
}
bool NemoInterface::Impl::_isSynchronized() const {
bool NemoInterface::Impl::_isSynchronizedNotSafe() const {
return _localTiles.size() == _remoteTiles.size() &&
std::equal(
_localTiles.begin(), _localTiles.end(), _remoteTiles.begin(),
[](const auto &a, const auto &b) { return a.first == b.first; });
}
void NemoInterface::Impl::_doAction() {
bool NemoInterface::Impl::_isSynchronized() const {
Lock lk(this->_m);
return _isSynchronizedNotSafe();
}
void NemoInterface::Impl::_doActionNotSafe() {
static bool resetDone = false;
switch (this->_state) {
case STATE::STOPPED:
_setWarningString("");
_setInfoString("");
this->_timeoutTimer.stop();
this->_restartTimer.stop();
this->_syncTimer.stop();
this->_clearTilesRemote(std::promise<bool>());
case STATE::STOPPED: {
std::promise<void> p;
INVM(this->_parent, ([this]() mutable {
this->_timeoutTimer.stop();
this->_restartTimer.stop();
this->_syncTimer.stop();
}))
if (this->_pRosbridge->state() != Rosbridge::STATE::STOPPED) {
this->_pRosbridge->stop();
}
_versionOK = false;
_progressTopicOK = false;
_heartbeatTopicOK = false;
break;
_dispatcher.stop();
_dispatcher.clear();
_setInfoStringNotSafe("");
_setWarningStringNotSafe("");
} break;
case STATE::START_BRIDGE: {
INVM(this->_parent,
([this]() mutable { this->_restartTimer.start(RESTART_INTERVAl); }))
case STATE::START_BRIDGE:
this->_pRosbridge->start();
this->_restartTimer.start(RESTART_INTERVAl);
break;
} break;
case STATE::WEBSOCKET_DETECTED:
resetDone = false;
this->_setState(STATE::TRY_SETUP);
this->_doAction();
this->_setStateNotSafe(STATE::TRY_SETUP);
this->_doActionNotSafe();
break;
case STATE::TRY_SETUP:
if (!_versionOK) {
this->_checkVersion();
} else if (!_progressTopicOK) {
this->_subscribeProgressTopic();
} else if (!_heartbeatTopicOK) {
this->_subscribeHearbeatTopic();
} else {
this->_timeoutTimer.start(NO_HEARTBEAT_TIMEOUT);
}
break;
case STATE::READY:
this->_trySynchronize();
this->_syncTimer.start(SYNC_INTERVAL);
this->_doSetup();
break;
case STATE::READY: {
INVM(this->_parent,
([this]() mutable { this->_syncTimer.start(SYNC_INTERVAL); }))
if (!_isSynchronizedNotSafe()) {
// can't call this here directly.
QTimer::singleShot(100, this->_parent, [this] { this->_synchronize(); });
}
_setInfoStringNotSafe("");
_setWarningStringNotSafe("");
} break;
case STATE::USER_SYNC:
case STATE::SYS_SYNC:
case STATE::SYNC_ERROR:
break;
case STATE::HEARTBEAT_TIMEOUT:
this->_clearTilesRemote(std::promise<bool>());
this->_syncTimer.stop();
break;
case STATE::WEBSOCKET_TIMEOUT:
case STATE::HEARTBEAT_TIMEOUT: {
INVM(this->_parent, ([this]() mutable { this->_syncTimer.stop(); }))
} break;
case STATE::WEBSOCKET_TIMEOUT: {
INVM(this->_parent, ([this]() mutable {
this->_timeoutTimer.stop();
this->_syncTimer.stop();
}))
if (!resetDone) {
resetDone = true;
this->_pRosbridge->stop();
this->_pRosbridge->start();
}
this->_timeoutTimer.stop();
this->_syncTimer.stop();
this->_clearTilesRemote(std::promise<bool>());
_versionOK = false;
_progressTopicOK = false;
_heartbeatTopicOK = false;
break;
_dispatcher.stop();
_dispatcher.clear();
_setInfoStringNotSafe("");
_setWarningStringNotSafe("");
} break;
};
}
QVariant NemoInterface::Impl::_callAddTiles(
std::shared_ptr<QVector<std::shared_ptr<const Tile>>> pTileArray) {
bool NemoInterface::Impl::_callAddTiles(
const QVector<std::shared_ptr<const Tile>> &tileArray, Dispatcher &d,
Rosbridge &rb) {
// qDebug() << "_callAddTiles called";
this->_lastCall = CALL_NAME::ADD_TILES;
// create json object
QJsonArray jsonTileArray;
for (auto &&tile : *pTileArray) {
for (auto &&tile : tileArray) {
using namespace ros_bridge::messages;
QJsonObject jsonTile;
if (!nemo_msgs::tile::toJson(*tile, jsonTile)) {
......@@ -989,13 +1080,13 @@ QVariant NemoInterface::Impl::_callAddTiles(
promise_response->set_value(o["success"].toBool());
} else {
qCWarning(NemoInterfaceLog)
<< "/nemo/add_tiles no \"success\" key or wrong type: " << o;
<< addTilesService << " no \"success\" key or wrong type: " << o;
promise_response->set_value(false);
}
};
// call service.
this->_pRosbridge->callService("/nemo/add_tiles", responseHandler, req);
rb.callService(addTilesService, responseHandler, req);
// wait for response.
auto tStart = hrc::now();
......@@ -1007,42 +1098,30 @@ QVariant NemoInterface::Impl::_callAddTiles(
break;
}
} while (hrc::now() - tStart < maxResponseTime ||
this->_dispatcher.isInterruptionRequested());
d.isInterruptionRequested());
if (abort) {
qCWarning(NemoInterfaceLog)
<< "addTiles(): Websocket not responding to request.";
return QVariant(false);
return false;
}
// transaction error?
if (!future_response.get()) {
return QVariant(false);
return false;
}
// add remote tiles (_remoteTiles)
std::promise<bool> promise;
auto future = promise.get_future();
bool value = QMetaObject::invokeMethod(
this->_parent /* context */,
[this, pTileArray, promise = std::move(promise)]() mutable {
this->_addTilesRemote(pTileArray, std::move(promise));
});
Q_ASSERT(value == true);
// return success
return QVariant(future.get());
return true;
}
QVariant
NemoInterface::Impl::_callRemoveTiles(std::shared_ptr<IDArray> pIdArray) {
bool NemoInterface::Impl::_callRemoveTiles(const IDArray &pIdArray,
Dispatcher &disp, Rosbridge &rb) {
// qDebug() << "_callRemoveTiles called";
this->_lastCall = CALL_NAME::REMOVE_TILES;
// create json object
QJsonArray jsonIdArray;
for (auto &&id : *pIdArray) {
for (auto &&id : pIdArray) {
using namespace ros_bridge::messages;
jsonIdArray.append(id);
} // for
......@@ -1059,13 +1138,13 @@ NemoInterface::Impl::_callRemoveTiles(std::shared_ptr<IDArray> pIdArray) {
promise_response->set_value(o["success"].toBool());
} else {
qCWarning(NemoInterfaceLog)
<< "/nemo/remove_tiles no \"success\" key or wrong type: " << msg;
<< removeTilesService << " no \"success\" key or wrong type: " << msg;
promise_response->set_value(false);
}
};
// call service.
this->_pRosbridge->callService("/nemo/remove_tiles", responseHandler, req);
rb.callService(removeTilesService, responseHandler, req);
// wait for response.
auto tStart = hrc::now();
......@@ -1077,37 +1156,23 @@ NemoInterface::Impl::_callRemoveTiles(std::shared_ptr<IDArray> pIdArray) {
break;
}
} while (hrc::now() - tStart < maxResponseTime ||
this->_dispatcher.isInterruptionRequested());
disp.isInterruptionRequested());
if (abort) {
qCWarning(NemoInterfaceLog)
<< "remove_tiles(): Websocket not responding to request.";
return QVariant(false);
return false;
}
// transaction error?
if (!future_response.get()) {
return QVariant(false);
return false;
}
// remove remote tiles (_remoteTiles)
std::promise<bool> promise;
auto future = promise.get_future();
bool value = QMetaObject::invokeMethod(
this->_parent /* context */,
[this, pIdArray, promise = std::move(promise)]() mutable {
this->_removeTilesRemote(pIdArray, std::move(promise));
});
Q_ASSERT(value == true);
// return success
return QVariant(future.get());
return true;
}
QVariant NemoInterface::Impl::_callClearTiles() {
// qDebug() << "_callClearTiles called";
this->_lastCall = CALL_NAME::CLEAR_TILES;
bool NemoInterface::Impl::_callClearTiles(Dispatcher &disp, Rosbridge &rb) {
// create response handler.
auto promise_response = std::make_shared<std::promise<bool>>();
......@@ -1118,8 +1183,7 @@ QVariant NemoInterface::Impl::_callClearTiles() {
};
// call service.
this->_pRosbridge->callService("/nemo/clear_tiles", responseHandler,
QJsonObject());
rb.callService(clearTilesService, responseHandler, QJsonObject());
// wait for response.
auto tStart = hrc::now();
......@@ -1131,40 +1195,29 @@ QVariant NemoInterface::Impl::_callClearTiles() {
break;
}
} while (hrc::now() - tStart < maxResponseTime ||
this->_dispatcher.isInterruptionRequested());
disp.isInterruptionRequested());
if (abort) {
qCWarning(NemoInterfaceLog) << "Websocket not responding to request.";
return QVariant(false);
return false;
}
// transaction failed?
if (!future_response.get()) {
return QVariant(false);
return false;
}
// clear remote tiles (_remoteTiles)
std::promise<bool> promise;
auto future = promise.get_future();
bool value = QMetaObject::invokeMethod(
this->_parent, [this, promise = std::move(promise)]() mutable {
this->_clearTilesRemote(std::move(promise));
});
Q_ASSERT(value == true);
// return success
return QVariant(future.get());
return true;
}
QVariant
NemoInterface::Impl::_callGetProgress(std::shared_ptr<IDArray> pIdArray) {
ProgressArray NemoInterface::Impl::_callGetProgress(const IDArray &pIdArray,
Dispatcher &disp,
Rosbridge &rb) {
// qDebug() << "_callGetProgress called";
this->_lastCall = CALL_NAME::GET_PROGRESS;
// create json object
QJsonArray jsonIdArray;
for (auto &&id : *pIdArray) {
for (auto &&id : pIdArray) {
using namespace ros_bridge::messages;
jsonIdArray.append(id);
} // for
......@@ -1185,15 +1238,15 @@ NemoInterface::Impl::_callGetProgress(std::shared_ptr<IDArray> pIdArray) {
*pArray = std::move(progressArrayMsg.progress_array());
promise_response->set_value(pArray);
} else {
qCWarning(NemoInterfaceLog)
<< "/nemo/get_progress error while creating ProgressArray "
"from json.";
qCWarning(NemoInterfaceLog) << getProgressService
<< " error while creating ProgressArray "
"from json.";
promise_response->set_value(nullptr);
}
};
// call service.
this->_pRosbridge->callService("/nemo/get_progress", responseHandler, req);
rb.callService(getProgressService, responseHandler, req);
// wait for response.
auto tStart = hrc::now();
......@@ -1205,38 +1258,22 @@ NemoInterface::Impl::_callGetProgress(std::shared_ptr<IDArray> pIdArray) {
break;
}
} while (hrc::now() - tStart < maxResponseTime ||
this->_dispatcher.isInterruptionRequested());
disp.isInterruptionRequested());
if (abort) {
qCWarning(NemoInterfaceLog)
<< "all_remove_tiles(): Websocket not responding to request.";
return QVariant(false);
}
// transaction error?
auto pArray = future_response.get();
if (pArray == nullptr) {
return QVariant(false);
return ProgressArray();
}
// remove remote tiles (_remoteTiles)
std::promise<bool> promise;
auto future = promise.get_future();
bool value = QMetaObject::invokeMethod(
this->_parent, [this, pArray, promise = std::move(promise)]() mutable {
this->_updateProgress(pArray, std::move(promise));
});
Q_ASSERT(value == true);
// return success
return QVariant(future.get());
return *future_response.get();
}
QVariant NemoInterface::Impl::_callGetAllProgress() {
ProgressArray NemoInterface::Impl::_callGetAllProgress(Dispatcher &disp,
Rosbridge &rb) {
// qDebug() << "_callGetAllProgress called";
this->_lastCall = CALL_NAME::GET_ALL_PROGRESS;
// create response handler.
typedef std::shared_ptr<ProgressArray> ResponseType;
auto promise_response = std::make_shared<std::promise<ResponseType>>();
......@@ -1251,17 +1288,16 @@ QVariant NemoInterface::Impl::_callGetAllProgress() {
*pArray = std::move(progressArrayMsg.progress_array());
promise_response->set_value(pArray);
} else {
qCWarning(NemoInterfaceLog)
<< "/nemo/get_all_progress error while creating ProgressArray "
"from json. msg: "
<< o;
qCWarning(NemoInterfaceLog) << getAllProgressService
<< " error while creating ProgressArray "
"from json. msg: "
<< o;
promise_response->set_value(nullptr);
}
};
// call service.
this->_pRosbridge->callService("/nemo/get_all_progress", responseHandler,
QJsonObject());
rb.callService(getAllProgressService, responseHandler, QJsonObject());
// wait for response.
auto tStart = hrc::now();
......@@ -1273,38 +1309,22 @@ QVariant NemoInterface::Impl::_callGetAllProgress() {
break;
}
} while (hrc::now() - tStart < maxResponseTime ||
this->_dispatcher.isInterruptionRequested());
disp.isInterruptionRequested());
if (abort) {
qCWarning(NemoInterfaceLog)
<< "all_remove_tiles(): Websocket not responding to request.";
return QVariant(false);
<< "_callGetAllProgress(): Websocket not responding to request.";
return ProgressArray();
}
// transaction error?
auto pArray = future_response.get();
if (pArray == nullptr) {
return QVariant(false);
}
// remove remote tiles (_remoteTiles)
std::promise<bool> promise;
auto future = promise.get_future();
bool value = QMetaObject::invokeMethod(
this->_parent, [this, pArray, promise = std::move(promise)]() mutable {
this->_updateProgress(pArray, std::move(promise));
});
Q_ASSERT(value == true);
// return success
return QVariant(future.get());
return *future_response.get();
}
QVariant NemoInterface::Impl::_callGetAllTiles() {
QVector<std::shared_ptr<Tile>>
NemoInterface::Impl::_callGetAllTiles(Dispatcher &disp, Rosbridge &rb) {
// qDebug() << "_callGetAllProgress called";
this->_lastCall = CALL_NAME::GET_ALL_TILES;
// create response handler.
typedef std::shared_ptr<QVector<std::shared_ptr<Tile>>> ResponseType;
auto promise_response = std::make_shared<std::promise<ResponseType>>();
......@@ -1324,12 +1344,12 @@ QVariant NemoInterface::Impl::_callGetAllTiles() {
pArray->push_back(tile);
} else {
qCWarning(NemoInterfaceLog)
<< "/nemo/get_all_tiles error while creating tile.";
<< getAllTilesService << " error while creating tile.";
promise_response->set_value(nullptr);
}
} else {
qCWarning(NemoInterfaceLog)
<< "/nemo/get_all_tiles json array does not contain objects.";
<< getAllTilesService << " json array does not contain objects.";
promise_response->set_value(nullptr);
}
}
......@@ -1337,14 +1357,13 @@ QVariant NemoInterface::Impl::_callGetAllTiles() {
promise_response->set_value(pArray);
} else {
qCWarning(NemoInterfaceLog)
<< "/nemo/get_all_tiles no tile_array key or wrong type.";
<< getAllTilesService << " no tile_array key or wrong type.";
promise_response->set_value(nullptr);
}
};
// call service.
this->_pRosbridge->callService("/nemo/get_all_tiles", responseHandler,
QJsonObject());
rb.callService(getAllTilesService, responseHandler, QJsonObject());
// wait for response.
auto tStart = hrc::now();
......@@ -1356,35 +1375,18 @@ QVariant NemoInterface::Impl::_callGetAllTiles() {
break;
}
} while (hrc::now() - tStart < maxResponseTime ||
this->_dispatcher.isInterruptionRequested());
disp.isInterruptionRequested());
if (abort) {
qCWarning(NemoInterfaceLog)
<< "all_remove_tiles(): Websocket not responding to request.";
return QVariant(false);
return QVector<std::shared_ptr<Tile>>();
}
// transaction error?
auto pArray = future_response.get();
if (pArray == nullptr) {
return QVariant(false);
}
// remote tiles (_remoteTiles)
std::promise<bool> promise;
auto future = promise.get_future();
bool value = QMetaObject::invokeMethod(
this->_parent, [this, pArray, promise = std::move(promise)]() mutable {
this->_compareAndSync(pArray, std::move(promise));
});
Q_ASSERT(value == true);
// return success
return QVariant(future.get());
return *future_response.get();
}
QVariant NemoInterface::Impl::_callGetVersion() {
this->_lastCall = CALL_NAME::GET_VERSION;
QString NemoInterface::Impl::_callGetVersion(Dispatcher &d, Rosbridge &rb) {
// create response handler.
typedef QString ResponseType;
......@@ -1399,14 +1401,13 @@ QVariant NemoInterface::Impl::_callGetVersion() {
promise_response->set_value(version);
} else {
qCWarning(NemoInterfaceLog)
<< "/nemo/get_version no version key or wrong type.";
<< getVersionService << " no version key or wrong type.";
promise_response->set_value("error!");
}
};
// call service.
this->_pRosbridge->callService("/nemo/get_version", responseHandler,
QJsonObject());
rb.callService(getVersionService, responseHandler, QJsonObject());
// wait for response.
auto tStart = hrc::now();
......@@ -1418,148 +1419,74 @@ QVariant NemoInterface::Impl::_callGetVersion() {
break;
}
} while (hrc::now() - tStart < maxResponseTime ||
this->_dispatcher.isInterruptionRequested());
d.isInterruptionRequested());
if (abort) {
qCWarning(NemoInterfaceLog)
<< "all_remove_tiles(): Websocket not responding to request.";
return QVariant(false);
return "unknown_version";
}
// transaction error?
auto version = future_response.get();
if (version == "error!") {
return QVariant(false);
return "unknown_version";
}
// remote tiles (_remoteTiles)
std::promise<bool> promise;
auto future = promise.get_future();
bool value = QMetaObject::invokeMethod(
this->_parent, [this, version, promise = std::move(promise)]() mutable {
this->_setVersion(version, std::move(promise));
});
Q_ASSERT(value == true);
// return success
return QVariant(future.get());
}
QString NemoInterface::Impl::_toString(NemoInterface::Impl::CALL_NAME name) {
switch (name) {
case CALL_NAME::ADD_TILES:
return QString("ADD_TILES");
case CALL_NAME::REMOVE_TILES:
return QString("REMOVE_TILES");
case CALL_NAME::CLEAR_TILES:
return QString("CLEAR_TILES");
case CALL_NAME::GET_PROGRESS:
return QString("GET_PROGRESS");
case CALL_NAME::GET_ALL_PROGRESS:
return QString("GET_ALL_PROGRESS");
case CALL_NAME::GET_ALL_TILES:
return QString("GET_ALL_TILES");
case CALL_NAME::GET_VERSION:
return QString("GET_VERSION");
}
return QString("unknown CALL_NAME");
return version;
}
void NemoInterface::Impl::_addTilesRemote(
std::shared_ptr<QVector<std::shared_ptr<const Tile>>> pTileArray,
std::promise<bool> promise) {
bool NemoInterface::Impl::_addTilesRemote(
const QVector<std::shared_ptr<const Tile>> tileArray) {
// qDebug() << "_addTilesRemote called";
auto pArrayDup = std::make_shared<QVector<std::shared_ptr<Tile>>>();
for (auto &&pTile : *pTileArray) {
pArrayDup->push_back(std::make_shared<Tile>(*pTile));
QVector<std::shared_ptr<Tile>> copy;
for (auto &&pTile : tileArray) {
copy.push_back(std::make_shared<Tile>(*pTile));
}
_addTilesRemote2(pArrayDup, std::move(promise));
return _addTilesRemote(copy);
}
void NemoInterface::Impl::_addTilesRemote2(
std::shared_ptr<QVector<std::shared_ptr<Tile>>> pTileArray,
std::promise<bool> promise) {
// qDebug() << "_addTilesRemote2 called";
bool NemoInterface::Impl::_addTilesRemote(
const QVector<std::shared_ptr<Tile>> &tileArray) {
bool error = false;
ProgressArray array;
bool anyChange = false;
for (auto &&pTile : *pTileArray) {
Lock lk(this->_m);
for (auto &&pTile : tileArray) {
auto id = pTile->id();
auto it = _remoteTiles.find(id);
if (Q_LIKELY(it == _remoteTiles.end())) {
auto ret = _remoteTiles.insert(std::make_pair(id, pTile));
array.push_back(LabeledProgress(pTile->progress(), pTile->id()));
anyChange = true;
Q_ASSERT(ret.second == true);
Q_UNUSED(ret);
} else {
qCWarning(NemoInterfaceLog)
<< "_addTilesRemote: tile with id " << id << " already added.";
if (pTile->tile() != it->second->tile()) {
qCWarning(NemoInterfaceLog)
<< "_addTilesRemote: tiles differ but have the same id.";
error = true;
}
}
}
if (array.size() > 0) {
if (this->_isSynchronized()) {
this->_setState(STATE::READY);
this->_doAction();
}
emit this->_parent->progressChanged(array);
}
promise.set_value(!error);
}
void NemoInterface::Impl::_compareAndSync(
std::shared_ptr<QVector<std::shared_ptr<Tile>>> pTileArray,
std::promise<bool> promise) {
bool synced = std::size_t(pTileArray->size()) == _localTiles.size();
if (synced) {
for (auto it = pTileArray->begin(); it != pTileArray->end(); ++it) {
auto match = _localTiles.find((*it)->id());
if (match == _localTiles.end()) {
synced = false;
}
}
}
if (!synced) {
qDebug() << "_compareAndSync: trying to synchronize";
_trySynchronize();
} else {
_remoteTiles.clear();
_addTilesRemote2(pTileArray, std::promise<bool>());
lk.unlock();
if (anyChange > 0) {
emit this->_parent->tilesChanged();
}
promise.set_value(true);
}
void NemoInterface::Impl::_setVersion(QString version,
std::promise<bool> promise) {
_remoteVersion = version;
if (_remoteVersion != _localVersion) {
_setWarningString("Version checking failed. Local protocol version (" +
_localVersion + ") does not match remote version (" +
_remoteVersion + ").");
} else {
_versionOK = true;
_doAction();
}
promise.set_value(true);
return !error;
}
void NemoInterface::Impl::_removeTilesRemote(std::shared_ptr<IDArray> idArray,
std::promise<bool> promise) {
void NemoInterface::Impl::_removeTilesRemote(const IDArray &idArray) {
// qDebug() << "_removeTilesRemote called";
bool anyChange = false;
for (auto &&id : *idArray) {
Lock lk(this->_m);
for (auto &&id : idArray) {
auto it = _remoteTiles.find(id);
if (Q_LIKELY(it != _remoteTiles.end())) {
_remoteTiles.erase(it);
......@@ -1571,30 +1498,25 @@ void NemoInterface::Impl::_removeTilesRemote(std::shared_ptr<IDArray> idArray,
}
if (anyChange) {
if (this->_isSynchronized()) {
this->_setState(STATE::READY);
this->_doAction();
}
emit this->_parent->tilesChanged();
}
}
promise.set_value(true);
void NemoInterface::Impl::_clearTilesRemote() {
Lock lk(this->_m);
_clearTilesRemoteNotSafe();
}
void NemoInterface::Impl::_clearTilesRemote(std::promise<bool> promise) {
void NemoInterface::Impl::_clearTilesRemoteNotSafe() {
// qDebug() << "_clearTilesRemote called";
if (_remoteTiles.size() > 0) {
_remoteTiles.clear();
if (this->_isSynchronized()) {
this->_setState(STATE::READY);
this->_doAction();
}
}
promise.set_value(true);
}
bool NemoInterface::Impl::_setState(STATE newState) {
bool NemoInterface::Impl::_setStateNotSafe(STATE newState) {
if (newState != this->_state) {
auto oldState = this->_state.load();
auto oldState = this->_state;
this->_state = newState;
qCDebug(NemoInterfaceLog)
......@@ -1602,11 +1524,15 @@ bool NemoInterface::Impl::_setState(STATE newState) {
auto oldStatus = _status(oldState);
auto newStatus = _status(newState);
if (oldStatus != newStatus) {
emit this->_parent->statusChanged();
// emit signal later
QTimer::singleShot(5, this->_parent,
[this] { emit this->_parent->statusChanged(); });
}
if (_running(oldState) != _running(newState)) {
emit this->_parent->runningChanged();
if (_runningNotSafe(oldState) != _runningNotSafe(newState)) {
// emit signal later
QTimer::singleShot(5, this->_parent,
[this] { emit this->_parent->runningChanged(); });
}
return true;
......@@ -1615,19 +1541,19 @@ bool NemoInterface::Impl::_setState(STATE newState) {
}
}
bool NemoInterface::Impl::_ready(NemoInterface::Impl::STATE s) {
bool NemoInterface::Impl::_readyNotSafe(NemoInterface::Impl::STATE s) {
return s == STATE::READY;
}
bool NemoInterface::Impl::_userSync(NemoInterface::Impl::STATE s) {
bool NemoInterface::Impl::_userSyncNotSafe(NemoInterface::Impl::STATE s) {
return s == STATE::USER_SYNC;
}
bool NemoInterface::Impl::_sysSync(NemoInterface::Impl::STATE s) {
bool NemoInterface::Impl::_sysSyncNotSafe(NemoInterface::Impl::STATE s) {
return s == STATE::SYS_SYNC;
}
bool NemoInterface::Impl::_running(NemoInterface::Impl::STATE s) {
bool NemoInterface::Impl::_runningNotSafe(NemoInterface::Impl::STATE s) {
return s != STATE::STOPPED;
}
......@@ -1654,6 +1580,9 @@ NemoInterface::Impl::_status(NemoInterface::Impl::STATE state) {
case STATE::SYS_SYNC:
status = NemoInterface::STATUS::SYNC;
break;
case STATE::SYNC_ERROR:
status = NemoInterface::STATUS::ERROR;
break;
case STATE::WEBSOCKET_TIMEOUT:
case STATE::HEARTBEAT_TIMEOUT:
status = NemoInterface::STATUS::TIMEOUT;
......@@ -1672,13 +1601,15 @@ QString NemoInterface::Impl::_toString(NemoInterface::Impl::STATE s) {
case STATE::WEBSOCKET_DETECTED:
return QString("WEBSOCKET_DETECTED");
case STATE::TRY_SETUP:
return QString("TRY_TOPIC_SERVICE_SETUP");
return QString("TRY_SETUP");
case STATE::READY:
return QString("READY");
case STATE::USER_SYNC:
return QString("SYNC_USER");
case STATE::SYS_SYNC:
return QString("SYNC_SYS");
case STATE::SYNC_ERROR:
return QString("SYNC_ERROR");
case STATE::WEBSOCKET_TIMEOUT:
return QString("WEBSOCKET_TIMEOUT");
case STATE::HEARTBEAT_TIMEOUT:
......@@ -1705,6 +1636,98 @@ QString NemoInterface::Impl::_toString(NemoInterface::STATUS s) {
return "unknown state!";
}
bool NemoInterface::Impl::_addTilesImpl(
std::shared_ptr<QVector<std::shared_ptr<const Tile>>> pTileArray,
std::shared_ptr<const IDArray> pIdArray) {
auto rb = this->_pRosbridge;
auto &disp = this->_dispatcher;
// add tiles
bool success = this->_callAddTiles(*pTileArray, disp, *rb);
if (!success) {
this->_setWarningString(
"Adding tiles failed. This might indicate a poor connection.");
return false;
}
auto ret = this->_addTilesRemote(*pTileArray);
if (!ret) {
Lock lk(this->_m);
this->_setStateNotSafe(STATE::SYNC_ERROR);
this->_doActionNotSafe();
lk.unlock();
this->_setWarningString("Adding tiles failed.");
qCDebug(NemoInterfaceLog) << "_addTilesImpl(): _addTilesRemote return "
"false: different tiles with same id.";
return false;
}
// fetch progress
auto array = this->_callGetProgress(*pIdArray, disp, *rb);
if (array.size() != pIdArray->size()) {
Lock lk(this->_m);
this->_setStateNotSafe(STATE::SYNC_ERROR);
this->_doActionNotSafe();
lk.unlock();
this->_setWarningString("Getting progress failed. This might "
"indicate a poor connection.");
return false;
}
ret = this->_updateProgress(array);
if (!ret) {
Lock lk(this->_m);
this->_setStateNotSafe(STATE::SYNC_ERROR);
this->_doActionNotSafe();
lk.unlock();
this->_setWarningString("Getting progress failed.");
qCDebug(NemoInterfaceLog)
<< "_addTilesImpl(): _updateProgress failed: unknown id.";
return false;
}
return true;
}
bool NemoInterface::Impl::_removeTilesImpl(
std::shared_ptr<const IDArray> pIdArray) {
auto rb = this->_pRosbridge;
auto &disp = this->_dispatcher;
auto success = this->_callRemoveTiles(*pIdArray, disp, *rb);
if (!success) {
Lock lk(this->_m);
this->_setStateNotSafe(STATE::SYNC_ERROR);
this->_doActionNotSafe();
lk.unlock();
this->_setWarningString("Removing tiles failed. This might "
"indicate a poor connection.");
return false;
}
this->_removeTilesRemote(*pIdArray);
return true;
}
bool NemoInterface::Impl::_clearTilesImpl() {
auto rb = this->_pRosbridge;
auto &disp = this->_dispatcher;
auto success = this->_callClearTiles(disp, *rb);
if (!success) {
Lock lk(this->_m);
this->_setStateNotSafe(STATE::SYNC_ERROR);
this->_doActionNotSafe();
lk.unlock();
this->_setWarningString("Clear tiles failed. This might "
"indicate a poor connection.");
return false;
}
this->_clearTilesRemote();
return true;
}
// ===============================================================
// NemoInterface
NemoInterface::NemoInterface()
......
......@@ -52,7 +52,11 @@ void TaskDispatcher::start() {
ULock lk1(this->_mutex);
if (!_running) {
this->_running = true;
_future = QtConcurrent::run([this]() mutable { return this->run(); });
auto p = std::make_shared<std::promise<void>>();
_threadFuture = p->get_future();
QtConcurrent::run([this, p = std::move(p)]() mutable {
return this->run(std::move(*p));
});
lk1.unlock();
}
}
......@@ -61,8 +65,8 @@ void TaskDispatcher::stop() {
ULock lk1(this->_mutex);
if (_running) {
this->_running = false;
_threadFuture.wait();
lk1.unlock();
this->_future.waitForFinished();
}
}
......@@ -88,7 +92,7 @@ std::size_t TaskDispatcher::pendingTasks() {
bool TaskDispatcher::idle() { return this->pendingTasks() == 0; }
void TaskDispatcher::run() {
void TaskDispatcher::run(std::promise<void> p) {
while (true) {
ULock lk1(this->_mutex);
......@@ -112,6 +116,7 @@ void TaskDispatcher::run() {
break;
}
}
p.set_value();
}
} // namespace nemo_interface
......@@ -56,13 +56,12 @@ public:
bool idle();
protected:
void run();
void run(std::promise<void> p);
private:
QFuture<void> _future;
std::deque<std::unique_ptr<Task>> _taskQueue;
std::deque<std::promise<QVariant>> _promiseQueue;
std::future<void> _threadFuture;
bool _running;
std::mutex _mutex;
};
......
......@@ -37,7 +37,7 @@ void Rosbridge::start() {
_impl->moveToThread(_t);
connect(_impl, &RosbridgeImpl::stateChanged, this,
&Rosbridge::stateChanged);
&Rosbridge::_onImplStateChanged);
connect(this, &Rosbridge::_start, _impl, &RosbridgeImpl::start);
connect(this, &Rosbridge::_stop, _impl, &RosbridgeImpl::stop);
......@@ -173,6 +173,26 @@ void Rosbridge::waitForTopic(const QString &topic) {
}
}
bool Rosbridge::waitForTopic(const QString &topic,
const std::function<bool()> &condition) {
std::future<bool> fut;
do {
fut = topicAvailable(topic);
// wait
while (true) {
auto status = fut.wait_for(std::chrono::milliseconds(5));
if (!condition()) {
return false;
}
if (status == std::future_status::ready) {
break;
}
}
} while (!fut.get());
return true;
}
void Rosbridge::advertiseService(const QString &service, const QString &type,
CallBackWReturn callback) {
emit _advertiseService(service, type, callback);
......@@ -244,6 +264,26 @@ void Rosbridge::waitForService(const QString &service) {
}
}
bool Rosbridge::waitForService(const QString &service,
const std::function<bool()> &condition) {
std::future<bool> fut;
do {
fut = serviceAvailable(service);
// wait
while (true) {
auto status = fut.wait_for(std::chrono::milliseconds(5));
if (!condition()) {
return false;
}
if (status == std::future_status::ready) {
break;
}
}
} while (!fut.get());
return true;
}
Rosbridge::STATE translate(RosbridgeImpl::STATE s) {
switch (s) {
case RosbridgeImpl::STATE::STOPPED:
......@@ -260,3 +300,4 @@ Rosbridge::STATE translate(RosbridgeImpl::STATE s) {
qCritical() << "unreachable branch!";
return Rosbridge::STATE::STOPPED;
}
void Rosbridge::_onImplStateChanged() { emit stateChanged(); }
......@@ -58,6 +58,8 @@ public:
void unsubscribeAllTopics();
void waitForTopic(const QString &topic);
bool waitForTopic(const QString &topic,
const std::function<bool()> &condition);
// Services.
void advertiseService(const QString &service, const QString &type,
......@@ -75,6 +77,8 @@ public:
std::future<QJsonObject> getAdvertisedServices();
void waitForService(const QString &service);
bool waitForService(const QString &service,
const std::function<bool()> &condition);
signals:
void stateChanged();
......@@ -99,6 +103,8 @@ signals:
const QJsonObject &args = QJsonObject());
void _unadvertiseService(const QString &service);
void _unadvertiseAllServices();
private slots:
void _onImplStateChanged();
private:
std::atomic<STATE> _state;
......
#include "routing.h"
#include "ortools/constraint_solver/routing.h"
#include "ortools/constraint_solver/routing_enums.pb.h"
#include "ortools/constraint_solver/routing_index_manager.h"
#include "ortools/constraint_solver/routing_parameters.h"
using namespace operations_research;
// Aux struct and functions.
struct InternalParameters {
InternalParameters()
: numSolutionsPerRun(1), numRuns(1), minNumTransectsPerRun(5),
stop([] { return false; }) {}
std::size_t numSolutionsPerRun;
std::size_t numRuns;
std::size_t minNumTransectsPerRun;
std::function<bool(void)> stop;
mutable std::string errorString;
};
bool getRoute(const FPolygon &area, const LineStringArray &transects,
std::vector<Solution> &solutionVector,
const InternalParameters &par = InternalParameters());
bool getRoute(const FPolygon &area, const LineStringArray &transects,
std::vector<Solution> &solutionVector,
const InternalParameters &par) {
#ifdef SNAKE_SHOW_TIME
auto start = std::chrono::high_resolution_clock::now();
#endif
//================================================================
// Create routing model.
//================================================================
// Use integer polygons to increase numerical robustness.
// Convert area;
IPolygon intArea;
for (const auto &v : area.outer()) {
auto p = float2Int(v);
intArea.outer().push_back(p);
}
for (const auto &ring : area.inners()) {
IRing intRing;
for (const auto &v : ring) {
auto p = float2Int(v);
intRing.push_back(p);
}
intArea.inners().push_back(std::move(intRing));
}
// Helper classes.
struct VirtualNode {
VirtualNode(std::size_t f, std::size_t t) : fromIndex(f), toIndex(t) {}
std::size_t fromIndex; // index for leaving node
std::size_t toIndex; // index for entering node
};
struct NodeToTransect {
NodeToTransect(std::size_t i, bool r) : transectsIndex(i), reversed(r) {}
std::size_t transectsIndex; // transects index
bool reversed; // transect reversed?
};
// Create vertex and node list
std::vector<IPoint> vertices;
std::vector<std::pair<std::size_t, std::size_t>> disjointNodes;
std::vector<VirtualNode> nodeList;
std::vector<NodeToTransect> nodeToTransectList;
for (std::size_t i = 0; i < transects.size(); ++i) {
const auto &t = transects[i];
// Copy line edges only.
if (t.size() == 1 || i == 0) {
auto p = float2Int(t.back());
vertices.push_back(p);
nodeToTransectList.emplace_back(i, false);
auto idx = vertices.size() - 1;
nodeList.emplace_back(idx, idx);
} else if (t.size() > 1) {
auto p1 = float2Int(t.front());
auto p2 = float2Int(t.back());
vertices.push_back(p1);
vertices.push_back(p2);
nodeToTransectList.emplace_back(i, false);
nodeToTransectList.emplace_back(i, true);
auto fromIdx = vertices.size() - 1;
auto toIdx = fromIdx - 1;
nodeList.emplace_back(fromIdx, toIdx);
nodeList.emplace_back(toIdx, fromIdx);
disjointNodes.emplace_back(toIdx, fromIdx);
} else { // transect empty
std::cout << "ignoring empty transect with index " << i << std::endl;
}
}
#ifdef SNAKE_DEBUG
// Print.
std::cout << "nodeToTransectList:" << std::endl;
std::cout << "node:transectIndex:reversed" << std::endl;
std::size_t c = 0;
for (const auto &n2t : nodeToTransectList) {
std::cout << c++ << ":" << n2t.transectsIndex << ":" << n2t.reversed
<< std::endl;
}
std::cout << "nodeList:" << std::endl;
std::cout << "node:fromIndex:toIndex" << std::endl;
c = 0;
for (const auto &n : nodeList) {
std::cout << c++ << ":" << n.fromIndex << ":" << n.toIndex << std::endl;
}
std::cout << "disjoint nodes:" << std::endl;
std::cout << "number:nodes" << std::endl;
c = 0;
for (const auto &d : disjointNodes) {
std::cout << c++ << ":" << d.first << "," << d.second << std::endl;
}
#endif
// Add polygon vertices.
for (auto &v : intArea.outer()) {
vertices.push_back(v);
}
for (auto &ring : intArea.inners()) {
for (auto &v : ring) {
vertices.push_back(v);
}
}
// Create connection graph (inf == no connection between vertices).
// Note: graph is not symmetric.
auto n = vertices.size();
// Matrix must be double since integers don't have infinity and nan
Matrix<double> connectionGraph(n, n);
for (std::size_t i = 0; i < n; ++i) {
auto &fromVertex = vertices[i];
for (std::size_t j = 0; j < n; ++j) {
auto &toVertex = vertices[j];
ILineString line{fromVertex, toVertex};
if (bg::covered_by(line, intArea)) {
connectionGraph(i, j) = bg::length(line);
} else {
connectionGraph(i, j) = std::numeric_limits<double>::infinity();
}
}
}
#ifdef SNAKE_DEBUG
std::cout << "connection grah:" << std::endl;
std::cout << connectionGraph << std::endl;
#endif
// Create distance matrix.
auto distLambda = [&connectionGraph](std::size_t i, std::size_t j) -> double {
return connectionGraph(i, j);
};
auto nNodes = nodeList.size();
Matrix<IntType> distanceMatrix(nNodes, nNodes);
for (std::size_t i = 0; i < nNodes; ++i) {
distanceMatrix(i, i) = 0;
for (std::size_t j = i + 1; j < nNodes; ++j) {
auto dist = connectionGraph(i, j);
if (std::isinf(dist)) {
std::vector<std::size_t> route;
if (!dijkstraAlgorithm(n, i, j, route, dist, distLambda)) {
std::stringstream ss;
ss << "Distance matrix calculation failed. connection graph: "
<< connectionGraph << std::endl;
ss << "area: " << bg::wkt(area) << std::endl;
ss << "transects:" << std::endl;
for (const auto &t : transects) {
ss << bg::wkt(t) << std::endl;
}
par.errorString = ss.str();
return false;
}
(void)route;
}
distanceMatrix(i, j) = dist;
distanceMatrix(j, i) = dist;
}
}
#ifdef SNAKE_DEBUG
std::cout << "distance matrix:" << std::endl;
std::cout << distanceMatrix << std::endl;
#endif
// Create (asymmetric) routing matrix.
Matrix<IntType> routingMatrix(nNodes, nNodes);
for (std::size_t i = 0; i < nNodes; ++i) {
auto fromNode = nodeList[i];
for (std::size_t j = 0; j < nNodes; ++j) {
auto toNode = nodeList[j];
routingMatrix(i, j) = distanceMatrix(fromNode.fromIndex, toNode.toIndex);
}
}
// Insert max for disjoint nodes.
for (const auto &d : disjointNodes) {
auto i = d.first;
auto j = d.second;
routingMatrix(i, j) = std::numeric_limits<IntType>::max();
routingMatrix(j, i) = std::numeric_limits<IntType>::max();
}
#ifdef SNAKE_DEBUG
std::cout << "routing matrix:" << std::endl;
std::cout << routingMatrix << std::endl;
#endif
// Create Routing Index Manager.
auto minNumTransectsPerRun =
std::max<std::size_t>(1, par.minNumTransectsPerRun);
auto maxRuns = std::max<std::size_t>(
1, std::floor(double(transects.size()) / minNumTransectsPerRun));
auto numRuns = std::max<std::size_t>(1, par.numRuns);
numRuns = std::min<std::size_t>(numRuns, maxRuns);
RoutingIndexManager::NodeIndex depot(0);
// std::vector<RoutingIndexManager::NodeIndex> depots(numRuns, depot);
// RoutingIndexManager manager(nNodes, numRuns, depots, depots);
RoutingIndexManager manager(nNodes, numRuns, depot);
// Create Routing Model.
RoutingModel routing(manager);
// Create and register a transit callback.
const int transitCallbackIndex = routing.RegisterTransitCallback(
[&routingMatrix, &manager](int64 from_index, int64 to_index) -> int64 {
// Convert from routing variable Index to distance matrix NodeIndex.
auto from_node = manager.IndexToNode(from_index).value();
auto to_node = manager.IndexToNode(to_index).value();
return routingMatrix(from_node, to_node);
});
// Define cost of each arc.
routing.SetArcCostEvaluatorOfAllVehicles(transitCallbackIndex);
// Add distance dimension.
if (numRuns > 1) {
routing.AddDimension(transitCallbackIndex, 0, 300000000,
true, // start cumul to zero
"Distance");
routing.GetMutableDimension("Distance")
->SetGlobalSpanCostCoefficient(100000000);
}
// Define disjunctions.
#ifdef SNAKE_DEBUG
std::cout << "disjunctions:" << std::endl;
#endif
for (const auto &d : disjointNodes) {
auto i = d.first;
auto j = d.second;
#ifdef SNAKE_DEBUG
std::cout << i << "," << j << std::endl;
#endif
auto idx0 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(i));
auto idx1 = manager.NodeToIndex(RoutingIndexManager::NodeIndex(j));
std::vector<int64> disj{idx0, idx1};
routing.AddDisjunction(disj, -1 /*force cardinality*/, 1 /*cardinality*/);
}
// Set first solution heuristic.
auto searchParameters = DefaultRoutingSearchParameters();
searchParameters.set_first_solution_strategy(
FirstSolutionStrategy::PATH_CHEAPEST_ARC);
// Number of solutions.
auto numSolutionsPerRun = std::max<std::size_t>(1, par.numSolutionsPerRun);
searchParameters.set_number_of_solutions_to_collect(numSolutionsPerRun);
// Set costume limit.
auto *solver = routing.solver();
auto *limit = solver->MakeCustomLimit(par.stop);
routing.AddSearchMonitor(limit);
#ifdef SNAKE_SHOW_TIME
auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start);
cout << "create routing model: " << delta.count() << " ms" << endl;
#endif
//================================================================
// Solve model.
//================================================================
#ifdef SNAKE_SHOW_TIME
start = std::chrono::high_resolution_clock::now();
#endif
auto pSolutions = std::make_unique<std::vector<const Assignment *>>();
(void)routing.SolveWithParameters(searchParameters, pSolutions.get());
#ifdef SNAKE_SHOW_TIME
delta = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start);
cout << "solve routing model: " << delta.count() << " ms" << endl;
#endif
if (par.stop()) {
par.errorString = "User terminated.";
return false;
}
if (pSolutions->size() == 0) {
std::stringstream ss;
ss << "No solution found." << std::endl;
par.errorString = ss.str();
return false;
}
//================================================================
// Construc route.
//================================================================
#ifdef SNAKE_SHOW_TIME
start = std::chrono::high_resolution_clock::now();
#endif
long long counter = -1;
// Note: route number 0 corresponds to the best route which is the last entry
// of *pSolutions.
for (auto solution = pSolutions->end() - 1; solution >= pSolutions->begin();
--solution) {
++counter;
if (!*solution || (*solution)->Size() <= 1) {
std::stringstream ss;
ss << par.errorString << "Solution " << counter << "invalid."
<< std::endl;
par.errorString = ss.str();
continue;
}
// Iterate over all routes.
Solution routeVector;
for (std::size_t vehicle = 0; vehicle < numRuns; ++vehicle) {
if (!routing.IsVehicleUsed(**solution, vehicle))
continue;
// Create index list.
auto index = routing.Start(vehicle);
std::vector<size_t> route_idx;
route_idx.push_back(manager.IndexToNode(index).value());
while (!routing.IsEnd(index)) {
index = (*solution)->Value(routing.NextVar(index));
route_idx.push_back(manager.IndexToNode(index).value());
}
#ifdef SNAKE_DEBUG
// Print route.
std::cout << "route " << counter
<< " route_idx.size() = " << route_idx.size() << std::endl;
std::cout << "route: ";
for (const auto &idx : route_idx) {
std::cout << idx << ", ";
}
std::cout << std::endl;
#endif
if (route_idx.size() < 2) {
std::stringstream ss;
ss << par.errorString
<< "Error while assembling route (solution = " << counter
<< ", run = " << vehicle << ")." << std::endl;
par.errorString = ss.str();
continue;
}
// Assemble route.
Route r;
auto &path = r.path;
auto &info = r.info;
for (size_t i = 0; i < route_idx.size() - 1; ++i) {
size_t nodeIndex0 = route_idx[i];
size_t nodeIndex1 = route_idx[i + 1];
const auto &n2t0 = nodeToTransectList[nodeIndex0];
info.emplace_back(n2t0.transectsIndex, n2t0.reversed);
// Copy transect to route.
const auto &t = transects[n2t0.transectsIndex];
if (n2t0.reversed) { // transect reversal needed?
for (auto it = t.end() - 1; it > t.begin(); --it) {
path.push_back(*it);
}
} else {
for (auto it = t.begin(); it < t.end() - 1; ++it) {
path.push_back(*it);
}
}
// Connect transects.
std::vector<size_t> idxList;
if (!shortestPathFromGraph(connectionGraph,
nodeList[nodeIndex0].fromIndex,
nodeList[nodeIndex1].toIndex, idxList)) {
std::stringstream ss;
ss << par.errorString
<< "Error while assembling route (solution = " << counter
<< ", run = " << vehicle << ")." << std::endl;
par.errorString = ss.str();
continue;
}
if (i != route_idx.size() - 2) {
idxList.pop_back();
}
for (auto idx : idxList) {
auto p = int2Float(vertices[idx]);
path.push_back(p);
}
}
// Append last transect info.
const auto &n2t0 = nodeToTransectList.back();
info.emplace_back(n2t0.transectsIndex, n2t0.reversed);
if (path.size() < 2 || info.size() < 2) {
std::stringstream ss;
ss << par.errorString << "Route empty (solution = " << counter
<< ", run = " << vehicle << ")." << std::endl;
par.errorString = ss.str();
continue;
}
routeVector.push_back(std::move(r));
}
if (routeVector.size() > 0) {
solutionVector.push_back(std::move(routeVector));
} else {
std::stringstream ss;
ss << par.errorString << "Solution " << counter << " empty." << std::endl;
par.errorString = ss.str();
}
}
#ifdef SNAKE_SHOW_TIME
delta = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::high_resolution_clock::now() - start);
cout << "reconstruct route: " << delta.count() << " ms" << endl;
#endif
if (solutionVector.size() > 0) {
return true;
} else {
return false;
}
}
#ifndef ROUTING_H
#define ROUTING_H
#endif // ROUTING_H
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