diff --git a/deploy/QGroundControl.AppImage b/deploy/QGroundControl.AppImage index d61944d8650c5851b1f1b4d273055cea76c42192..50e0ab09c02277aa28919569fbd073e961edbf11 100755 Binary files a/deploy/QGroundControl.AppImage and b/deploy/QGroundControl.AppImage differ diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 0e70af2e633a77e7259cf4efb9ac80da1bf8a410..656a58a52a4dcc5fa0b6fa3a43e0c1101ef8bd89 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -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 \ diff --git a/src/MeasurementComplexItem/NemoInterface.cpp b/src/MeasurementComplexItem/NemoInterface.cpp index f0898dda5c3e0003e81a6f3c907455fa9be005e4..dfb28f2c4bb0dee1f0f49536da6c24c58be4a939 100644 --- a/src/MeasurementComplexItem/NemoInterface.cpp +++ b/src/MeasurementComplexItem/NemoInterface.cpp @@ -7,7 +7,7 @@ #include "SettingsManager.h" #include "WimaSettings.h" -#include +#include #include #include @@ -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 requiredServices{ + addTilesService, removeTilesService, clearTilesService, + getAllTilesService, getAllProgressService, getProgressService, + getVersionService}; + +static const std::vector requiredTopics{progressTopic, + heartbeatTopic}; using hrc = std::chrono::high_resolution_clock; using ROSBridgePtr = std::shared_ptr; @@ -51,8 +96,7 @@ typedef std::map> TileMap; typedef std::map> TileMapConst; typedef ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat Heartbeat; typedef nemo_interface::TaskDispatcher Dispatcher; -typedef nemo_interface::FutureWatcher - FutureWatcher; +typedef std::unique_lock 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>> pTileArray); - // called from dispatcher thread! - QVariant _callRemoveTiles(std::shared_ptr pIdArray); - // called from dispatcher thread! - QVariant _callClearTiles(); - // called from dispatcher thread! - QVariant _callGetProgress(std::shared_ptr 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>> pTileArray, - std::promise promise); - void - _addTilesRemote2(std::shared_ptr>> pTileArray, - std::promise promise); - void - _compareAndSync(std::shared_ptr>> pTileArray, - std::promise promise); - void _setVersion(QString version, std::promise promise); - void _removeTilesRemote(std::shared_ptr idArray, - std::promise promise); - void _clearTilesRemote(std::promise promise); - void _updateProgress(std::shared_ptr pArray, - std::promise promise); - void _onHeartbeatReceived(const QNemoHeartbeat &hb, - std::promise 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>> pTileArray, + std::shared_ptr pIdArray); + bool _removeTilesImpl(std::shared_ptr pIdArray); + bool _clearTilesImpl(); + + // Worker functions. + static bool + _callAddTiles(const QVector> &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> _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> tileArray); + bool _addTilesRemote(const QVector> &tileArray); + void _removeTilesRemote(const IDArray &idArray); + void _clearTilesRemote(); + void _clearTilesRemoteNotSafe(); + + // static and const members + static const char *_localVersion; + NemoInterface *const _parent; - std::atomic _state; - std::atomic_bool _versionOK; - std::atomic_bool _progressTopicOK; - std::atomic_bool _heartbeatTopicOK; - std::atomic _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; 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(); }); } 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>>(); auto pIdArray = std::make_shared(); + + 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( - 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([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([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(); + + 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( - std::bind(&Impl::_callRemoveTiles, this, pIdArray)); + auto cmd = std::make_unique([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 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(std::bind(&Impl::_callClearTiles, this)); + auto pTask = std::make_unique([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 p; p.set_value(QVariant(false)); return p.get_future(); @@ -410,6 +475,7 @@ std::shared_future 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 pArray, - std::promise 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 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([this] { - // wait for service - std::future 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([this] { - // wait for service - std::future 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(); - *p = std::move(progressArray.progress_array()); - std::promise 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([this] { - // wait for service - std::future 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 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>>(); + for (auto it = _localTiles.begin(); it != _localTiles.end(); ++it) { + pTileArray->push_back(it->second); + } + lk.unlock(); + + // create cmd. + auto pTask = std::make_unique([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> 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( - 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(); + for (auto &&pTile : *pTileArray) { + pIdArray->push_back(pTile->id()); + } - // create tile array. - auto pTileArray = std::make_shared>>(); - for (const auto &pair : _localTiles) { - pTileArray->push_back(pair.second); - } + success = this->_addTilesImpl(pTileArray, pIdArray); - // create addTiles cmd. - pTask = - std::make_unique([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([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( - [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()); + case STATE::STOPPED: { + std::promise 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()); - 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()); - _versionOK = false; - _progressTopicOK = false; - _heartbeatTopicOK = false; - break; + _dispatcher.stop(); + _dispatcher.clear(); + + _setInfoStringNotSafe(""); + _setWarningStringNotSafe(""); + } break; }; } -QVariant NemoInterface::Impl::_callAddTiles( - std::shared_ptr>> pTileArray) { +bool NemoInterface::Impl::_callAddTiles( + const QVector> &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 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 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 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 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 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>(); @@ -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 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 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 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 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 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 ResponseType; auto promise_response = std::make_shared>(); @@ -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 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> +NemoInterface::Impl::_callGetAllTiles(Dispatcher &disp, Rosbridge &rb) { // qDebug() << "_callGetAllProgress called"; - this->_lastCall = CALL_NAME::GET_ALL_TILES; - // create response handler. typedef std::shared_ptr>> ResponseType; auto promise_response = std::make_shared>(); @@ -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>(); } - // transaction error? - auto pArray = future_response.get(); - if (pArray == nullptr) { - return QVariant(false); - } - - // remote tiles (_remoteTiles) - std::promise 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 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>> pTileArray, - std::promise promise) { +bool NemoInterface::Impl::_addTilesRemote( + const QVector> tileArray) { // qDebug() << "_addTilesRemote called"; - auto pArrayDup = std::make_shared>>(); - for (auto &&pTile : *pTileArray) { - pArrayDup->push_back(std::make_shared(*pTile)); + QVector> copy; + for (auto &&pTile : tileArray) { + copy.push_back(std::make_shared(*pTile)); } - _addTilesRemote2(pArrayDup, std::move(promise)); + return _addTilesRemote(copy); } -void NemoInterface::Impl::_addTilesRemote2( - std::shared_ptr>> pTileArray, - std::promise promise) { - - // qDebug() << "_addTilesRemote2 called"; +bool NemoInterface::Impl::_addTilesRemote( + const QVector> &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>> pTileArray, - std::promise 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()); + lk.unlock(); + if (anyChange > 0) { + emit this->_parent->tilesChanged(); } - promise.set_value(true); -} - -void NemoInterface::Impl::_setVersion(QString version, - std::promise 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, - std::promise 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, } 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 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>> pTileArray, + std::shared_ptr 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 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() diff --git a/src/MeasurementComplexItem/nemo_interface/TaskDispatcher.cpp b/src/MeasurementComplexItem/nemo_interface/TaskDispatcher.cpp index 22697a44e21a9317da3b47e6beec6bffc59de3ff..3f1cfa2f55c60dae9c8afab274e907aa98707e49 100644 --- a/src/MeasurementComplexItem/nemo_interface/TaskDispatcher.cpp +++ b/src/MeasurementComplexItem/nemo_interface/TaskDispatcher.cpp @@ -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>(); + _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 p) { while (true) { ULock lk1(this->_mutex); @@ -112,6 +116,7 @@ void TaskDispatcher::run() { break; } } + p.set_value(); } } // namespace nemo_interface diff --git a/src/MeasurementComplexItem/nemo_interface/TaskDispatcher.h b/src/MeasurementComplexItem/nemo_interface/TaskDispatcher.h index 20dee8a33aa6856e14d2b6902141b1c1ce2bac87..3c000b18cb0e3e7da4e7a1f5e201f0a61ad11e9c 100644 --- a/src/MeasurementComplexItem/nemo_interface/TaskDispatcher.h +++ b/src/MeasurementComplexItem/nemo_interface/TaskDispatcher.h @@ -56,13 +56,12 @@ public: bool idle(); protected: - void run(); + void run(std::promise p); private: - QFuture _future; - std::deque> _taskQueue; std::deque> _promiseQueue; + std::future _threadFuture; bool _running; std::mutex _mutex; }; diff --git a/src/MeasurementComplexItem/rosbridge/rosbridge.cpp b/src/MeasurementComplexItem/rosbridge/rosbridge.cpp index 6290445af9d1cfc0f09e4502338384e3520ae9be..4b04c3edc1ad89f97b47c69a9405c354f2cec586 100644 --- a/src/MeasurementComplexItem/rosbridge/rosbridge.cpp +++ b/src/MeasurementComplexItem/rosbridge/rosbridge.cpp @@ -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 &condition) { + std::future 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 &condition) { + std::future 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(); } diff --git a/src/MeasurementComplexItem/rosbridge/rosbridge.h b/src/MeasurementComplexItem/rosbridge/rosbridge.h index f3d70354ee2e5a153cecd390d79fdafa2bdb17ef..74617131dc60c5201b61755578083c3386530f28 100644 --- a/src/MeasurementComplexItem/rosbridge/rosbridge.h +++ b/src/MeasurementComplexItem/rosbridge/rosbridge.h @@ -58,6 +58,8 @@ public: void unsubscribeAllTopics(); void waitForTopic(const QString &topic); + bool waitForTopic(const QString &topic, + const std::function &condition); // Services. void advertiseService(const QString &service, const QString &type, @@ -75,6 +77,8 @@ public: std::future getAdvertisedServices(); void waitForService(const QString &service); + bool waitForService(const QString &service, + const std::function &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; diff --git a/src/MeasurementComplexItem/routing.cpp b/src/MeasurementComplexItem/routing.cpp deleted file mode 100644 index 4905ec4b6b66233f0e21e5d0d57b95180af67711..0000000000000000000000000000000000000000 --- a/src/MeasurementComplexItem/routing.cpp +++ /dev/null @@ -1,423 +0,0 @@ -#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 stop; - mutable std::string errorString; -}; - -bool getRoute(const FPolygon &area, const LineStringArray &transects, - std::vector &solutionVector, - const InternalParameters &par = InternalParameters()); - - -bool getRoute(const FPolygon &area, const LineStringArray &transects, - std::vector &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 vertices; - std::vector> disjointNodes; - std::vector nodeList; - std::vector 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 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::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 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 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 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::max(); - routingMatrix(j, i) = std::numeric_limits::max(); - } -#ifdef SNAKE_DEBUG - std::cout << "routing matrix:" << std::endl; - std::cout << routingMatrix << std::endl; -#endif - - // Create Routing Index Manager. - auto minNumTransectsPerRun = - std::max(1, par.minNumTransectsPerRun); - auto maxRuns = std::max( - 1, std::floor(double(transects.size()) / minNumTransectsPerRun)); - auto numRuns = std::max(1, par.numRuns); - numRuns = std::min(numRuns, maxRuns); - - RoutingIndexManager::NodeIndex depot(0); - // std::vector 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 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(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::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>(); - (void)routing.SolveWithParameters(searchParameters, pSolutions.get()); -#ifdef SNAKE_SHOW_TIME - delta = std::chrono::duration_cast( - 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 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 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::high_resolution_clock::now() - start); - cout << "reconstruct route: " << delta.count() << " ms" << endl; -#endif - - if (solutionVector.size() > 0) { - return true; - } else { - return false; - } -} diff --git a/src/MeasurementComplexItem/routing.h b/src/MeasurementComplexItem/routing.h deleted file mode 100644 index a2dbe64076e6c7707cc4890e95905695d20eaf58..0000000000000000000000000000000000000000 --- a/src/MeasurementComplexItem/routing.h +++ /dev/null @@ -1,4 +0,0 @@ -#ifndef ROUTING_H -#define ROUTING_H - -#endif // ROUTING_H