From 22c1b55ececf42a4f90cbaab34234dd9e97a7ea7 Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Tue, 19 Jan 2021 18:27:58 +0100 Subject: [PATCH] temp --- qgroundcontrol.pro | 11 +- .../MeasurementComplexItem.cc | 73 ++ .../MeasurementComplexItem.h | 3 + src/MeasurementComplexItem/NemoInterface.cpp | 1140 ++++++++++++++--- src/MeasurementComplexItem/NemoInterface.h | 29 +- .../geometry/GeoArea.cc | 15 +- .../geometry/MeasurementArea.cc | 129 +- .../geometry/MeasurementArea.h | 6 +- .../geometry/ProgressArray.h | 7 +- .../nemo_interface/CommandDispatcher.cpp | 7 - .../nemo_interface/CommandDispatcher.h | 31 - .../nemo_interface/FutureWatcher.h | 74 ++ .../nemo_interface/FutureWatcherInterface.cpp | 5 + .../nemo_interface/FutureWatcherInterface.h | 23 + .../nemo_interface/MeasurementTile.cpp | 7 +- .../nemo_interface/MeasurementTile.h | 6 +- .../nemo_interface/Task.cpp | 12 +- .../nemo_interface/Task.h | 14 +- .../nemo_interface/TaskDispatcher.cpp | 111 ++ .../nemo_interface/TaskDispatcher.h | 70 + .../ros_bridge/include/RosBridgeClient.cpp | 3 +- .../include/messages/nemo_msgs/heartbeat.h | 1 - .../messages/nemo_msgs/labeled_progress.h | 6 +- .../messages/nemo_msgs/progress_array.cpp | 5 + .../messages/nemo_msgs/progress_array.h | 88 ++ .../include/messages/nemo_msgs/tile.h | 2 +- 26 files changed, 1534 insertions(+), 344 deletions(-) delete mode 100644 src/MeasurementComplexItem/nemo_interface/CommandDispatcher.cpp delete mode 100644 src/MeasurementComplexItem/nemo_interface/CommandDispatcher.h create mode 100644 src/MeasurementComplexItem/nemo_interface/FutureWatcher.h create mode 100644 src/MeasurementComplexItem/nemo_interface/FutureWatcherInterface.cpp create mode 100644 src/MeasurementComplexItem/nemo_interface/FutureWatcherInterface.h create mode 100644 src/MeasurementComplexItem/nemo_interface/TaskDispatcher.cpp create mode 100644 src/MeasurementComplexItem/nemo_interface/TaskDispatcher.h create mode 100644 src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.cpp create mode 100644 src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 606d00bf2..2a7ea35ac 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -35,7 +35,7 @@ DebugBuild { #DEFINES += SNAKE_SHOW_TIME #DEFINES += SNAKE_DEBUG #DEFINES += SNAKE_SHOW_TIME - DEFINES += ROS_BRIDGE_DEBUG + #DEFINES += ROS_BRIDGE_DEBUG } else { DESTDIR = $${OUT_PWD}/release @@ -453,10 +453,12 @@ HEADERS += \ src/MeasurementComplexItem/geometry/TileDiff.h \ src/MeasurementComplexItem/geometry/geometry.h \ src/MeasurementComplexItem/HashFunctions.h \ + src/MeasurementComplexItem/nemo_interface/FutureWatcher.h \ + src/MeasurementComplexItem/nemo_interface/FutureWatcherInterface.h \ src/MeasurementComplexItem/nemo_interface/Task.h \ + src/MeasurementComplexItem/nemo_interface/TaskDispatcher.h \ src/MeasurementComplexItem/nemo_interface/tileHelper.h \ src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h \ - src/MeasurementComplexItem/nemo_interface/CommandDispatcher.h \ src/MeasurementComplexItem/nemo_interface/MeasurementTile.h \ src/QmlControls/QmlUnitsConversion.h \ src/MeasurementComplexItem/geometry/GeoArea.h \ @@ -510,6 +512,7 @@ HEADERS += \ src/comm/ros_bridge/include/message_traits.h \ src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h \ src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h \ + src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.h \ src/comm/ros_bridge/include/messages/nemo_msgs/tile.h \ src/comm/ros_bridge/include/messages/std_msgs/header.h \ src/comm/ros_bridge/include/server.h \ @@ -529,9 +532,10 @@ SOURCES += \ src/MeasurementComplexItem/geometry/SafeArea.cc \ src/MeasurementComplexItem/geometry/geometry.cpp \ src/MeasurementComplexItem/HashFunctions.cpp \ - src/MeasurementComplexItem/nemo_interface/CommandDispatcher.cpp \ + src/MeasurementComplexItem/nemo_interface/FutureWatcherInterface.cpp \ src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp \ src/MeasurementComplexItem/nemo_interface/Task.cpp \ + src/MeasurementComplexItem/nemo_interface/TaskDispatcher.cpp \ src/Vehicle/VehicleEscStatusFactGroup.cc \ src/MeasurementComplexItem/AreaData.cc \ src/api/QGCCorePlugin.cc \ @@ -553,6 +557,7 @@ SOURCES += \ src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.cpp \ src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.cpp \ src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.cpp \ + src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.cpp \ src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp \ src/comm/ros_bridge/include/messages/std_msgs/header.cpp \ src/comm/ros_bridge/include/messages/std_msgs/time.cpp \ diff --git a/src/MeasurementComplexItem/MeasurementComplexItem.cc b/src/MeasurementComplexItem/MeasurementComplexItem.cc index 06bb3babf..d89b8e6e9 100644 --- a/src/MeasurementComplexItem/MeasurementComplexItem.cc +++ b/src/MeasurementComplexItem/MeasurementComplexItem.cc @@ -2,6 +2,7 @@ #include "CircularGenerator.h" #include "LinearGenerator.h" +#include "NemoInterface.h" #include "RoutingThread.h" #include "geometry/GenericCircle.h" #include "geometry/MeasurementArea.h" @@ -128,6 +129,10 @@ MeasurementComplexItem::MeasurementComplexItem( resetGenerators(); startEditing(); + + // connect to nemo interface + connect(pNemoInterface, &NemoInterface::progressChanged, this, + &MeasurementComplexItem::_onNewProgress); } MeasurementComplexItem::~MeasurementComplexItem() {} @@ -919,6 +924,69 @@ void MeasurementComplexItem::_reverseRoute() { } } +void MeasurementComplexItem::_syncTiles() { + auto areaArray = _pAreaData->measurementAreaArray(); + bool clear = false; + if (areaArray.size() > 0) { + + // create tile ptr array + TilePtrArray tilePtrArray; + auto *pMeasurementArea = areaArray[0]; + auto pTiles = pMeasurementArea->tiles(); + for (int i = 0; i < pTiles->count(); ++i) { + auto *tile = pTiles->value(i); + Q_ASSERT(tile != nullptr); + tilePtrArray.push_back(tile); + } + + if (tilePtrArray.size() > 0) { + + // create id array + IDArray idArray; + for (const auto *pTile : tilePtrArray) { + idArray.push_back(pTile->id()); + } + + // sync. necessary? + bool doSync = false; + auto contains = pNemoInterface->containsTiles(idArray); + for (auto &&logical : contains) { + if (logical == false) { + doSync = true; + break; + } + } + + if (doSync) { + if (!pNemoInterface->empty()) { + (void)pNemoInterface->clearTiles(); + } + (void)pNemoInterface->addTiles(tilePtrArray); + return; + } + } else { + clear = true; + } + } else { + clear = true; + } + + if (clear) { + if (!pNemoInterface->empty()) { + (void)pNemoInterface->clearTiles(); + } + } +} + +void MeasurementComplexItem::_onNewProgress(const ProgressArray &array) { + auto areaArray = this->_pAreaData->measurementAreaArray(); + if (areaArray.size() > 0) { + for (auto &area : areaArray) { + area->updateProgress(array); + } + } +} + ComplexMissionItem::ReadyForSaveState MeasurementComplexItem::readyForSaveState() const { if (idle()) { @@ -1145,6 +1213,10 @@ bool MeasurementComplexItem::stopEditing(bool doUpdate) { _updateRoute(); } + if (correct && isDifferent) { + _syncTiles(); + } + return updated; } return false; @@ -1153,6 +1225,7 @@ bool MeasurementComplexItem::stopEditing(bool doUpdate) { void MeasurementComplexItem::abortEditing() { if (editing()) { _setAreaData(_pAreaData); + _syncTiles(); _setState(STATE::IDLE); } } diff --git a/src/MeasurementComplexItem/MeasurementComplexItem.h b/src/MeasurementComplexItem/MeasurementComplexItem.h index ab71fa1da..826811e1f 100644 --- a/src/MeasurementComplexItem/MeasurementComplexItem.h +++ b/src/MeasurementComplexItem/MeasurementComplexItem.h @@ -10,6 +10,7 @@ #include "SettingsFact.h" #include "AreaData.h" +#include "ProgressArray.h" class RoutingThread; class RoutingResult; @@ -238,6 +239,8 @@ private slots: void _updateRoute(); void _changeVariantIndex(); void _reverseRoute(); + void _syncTiles(); + void _onNewProgress(const ProgressArray &array); private: bool _setGenerator(PtrGenerator newG); diff --git a/src/MeasurementComplexItem/NemoInterface.cpp b/src/MeasurementComplexItem/NemoInterface.cpp index 9e887d5d7..6864cc098 100644 --- a/src/MeasurementComplexItem/NemoInterface.cpp +++ b/src/MeasurementComplexItem/NemoInterface.cpp @@ -14,9 +14,12 @@ #include "GenericSingelton.h" #include "geometry/MeasurementArea.h" #include "geometry/geometry.h" -#include "nemo_interface/CommandDispatcher.h" +#include "nemo_interface/FutureWatcher.h" #include "nemo_interface/MeasurementTile.h" #include "nemo_interface/QNemoHeartbeat.h" +#include "nemo_interface/TaskDispatcher.h" + +#include "ros_bridge/include/messages/nemo_msgs/progress_array.h" #include "ros_bridge/include/RosBridgeClient.h" #include "ros_bridge/include/messages/geographic_msgs/geopoint.h" @@ -29,14 +32,23 @@ QGC_LOGGING_CATEGORY(NemoInterfaceLog, "NemoInterfaceLog") -#define EVENT_TIMER_INTERVAL 100 // ms -#define NO_HEARTBEAT_TIMEOUT 5000 // ms -auto constexpr static maxResponseTime = std::chrono::milliseconds(10000); -using hrc = std::chrono::high_resolution_clock; +#define SYNC_INTERVAL 1000 // ms +#define NO_HEARTBEAT_TIMEOUT 10000 // ms +#define CONNECTION_TIMER_INTERVAL 1000 // ms +static constexpr auto maxResponseTime = std::chrono::milliseconds(10000); +using hrc = std::chrono::high_resolution_clock; using ROSBridgePtr = std::shared_ptr; -using UniqueLock = std::unique_lock; -using SharedLock = std::shared_lock; + +typedef ros_bridge::messages::nemo_msgs::tile::GenericTile + Tile; +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; class NemoInterface::Impl { enum class STATE { @@ -44,8 +56,11 @@ class NemoInterface::Impl { START_BRIDGE, WEBSOCKET_DETECTED, TRY_TOPIC_SERVICE_SETUP, + SYNC_USER, + SYNC_SYS, READY, - TIMEOUT + WEBSOCKET_TIMEOUT, + HEARTBEAT_TIMEOUT }; public: @@ -57,9 +72,9 @@ public: // Tile editing. // Functions that require communication to device. - std::future addTiles(const TileArray &tileArray); - void removeTiles(const IDArray &idArray); - void clearTiles(); + std::shared_future addTiles(const TilePtrArray &tileArray); + std::shared_future removeTiles(const IDArray &idArray); + std::shared_future clearTiles(); // Functions that don't require communication to device. TileArray getTiles(const IDArray &idArray); @@ -79,51 +94,69 @@ public: const QString &infoString(); const QString &warningString(); - void _addTilesLocal(const TileArray &tileArray); - void _removeTilesLocal(const IDArray &idArray); - void _clearTilesLocal(); - void _updateProgress(ProgressArray progressArray); - void _setHeartbeat(const QNemoHeartbeat &hb); - void _setInfoString(const QString &info); - void _setWarningString(const QString &warning); - private: - typedef std::chrono::time_point TimePoint; - typedef std::map TileMap; - typedef ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat Heartbeat; - typedef nemo_interface::CommandDispatcher Dispatcher; - void _doTopicServiceSetup(); - void _doAction(); // does action according to state + void _doAction(); + void _trySynchronize(); + bool _isSynchronized(); + bool _userSync(); // thread safe + bool _sysSync(); // thread safe + void _onFutureWatcherFinished(); // thread safe + + // 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); + + void _addTilesRemote( + std::shared_ptr>> pTileArray); + void + _addTilesRemote(std::shared_ptr>> pTileArray); + void _removeTilesRemote(std::shared_ptr idArray); + void _clearTilesRemote(); + void _updateProgress(std::shared_ptr pArray); + void _onHeartbeatReceived(const QNemoHeartbeat &hb); + void _setInfoString(const QString &info); + void _setWarningString(const QString &warning); - bool _setState(STATE s); // not thread safe + 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); - static void _translate(STATE state, NemoInterface::STATUS &status); - static void _translate(Heartbeat hb, STATE &state); + static NemoInterface::STATUS _status(STATE state); + static QString _toString(STATE s); + static QString _toString(NemoInterface::STATUS s); std::atomic _state; ROSBridgePtr _pRosBridge; - TileMap _tileMap; - NemoInterface *_parent; + TileMap _remoteTiles; + TileMapConst _localTiles; + NemoInterface *const _parent; Dispatcher _dispatcher; QString _infoString; QString _warningString; QTimer _timeoutTimer; QTimer _connectionTimer; QNemoHeartbeat _lastHeartbeat; + FutureWatcher _futureWatcher; }; using StatusMap = std::map; static StatusMap statusMap{ std::make_pair( NemoInterface::STATUS::NOT_CONNECTED, "Not Connected"), + std::make_pair(NemoInterface::STATUS::SYNC, + "Synchronizing"), std::make_pair(NemoInterface::STATUS::READY, "Ready"), std::make_pair( NemoInterface::STATUS::TIMEOUT, "Timeout"), - std::make_pair( - NemoInterface::STATUS::INVALID_HEARTBEAT, "Error"), std::make_pair( NemoInterface::STATUS::WEBSOCKET_DETECTED, "Websocket Detected")}; @@ -150,32 +183,38 @@ NemoInterface::Impl::Impl(NemoInterface *p) this->_pRosBridge = std::make_shared( connectionString.toLocal8Bit().data()); this->_pRosBridge->reset(); - qCritical() << "NemoInterface: add reset code here"; }; connect(connectionStringFact, &SettingsFact::rawValueChanged, setConnectionString); setConnectionString(); // Heartbeat timeout. - connect(&this->_timeoutTimer, &QTimer::timeout, - [this] { this->_setState(STATE::TIMEOUT); }); + connect(&this->_timeoutTimer, &QTimer::timeout, [this] { + this->_setState(STATE::HEARTBEAT_TIMEOUT); + this->_doAction(); + }); // Connection timer (temporary workaround) connect(&this->_connectionTimer, &QTimer::timeout, [this] { if (this->_pRosBridge->connected()) { if (this->_state == STATE::START_BRIDGE || - this->_state == STATE::TIMEOUT) { + this->_state == STATE::WEBSOCKET_TIMEOUT) { this->_setState(STATE::WEBSOCKET_DETECTED); this->_doAction(); } } else { if (this->_state == STATE::TRY_TOPIC_SERVICE_SETUP || - this->_state == STATE::READY) { - this->_setState(STATE::TIMEOUT); + this->_state == STATE::READY || + this->_state == STATE::WEBSOCKET_DETECTED || + this->_state == STATE::HEARTBEAT_TIMEOUT) { + this->_setState(STATE::WEBSOCKET_TIMEOUT); this->_doAction(); } } }); + + connect(&this->_futureWatcher, &FutureWatcher::finished, + [this] { this->_onFutureWatcherFinished(); }); } NemoInterface::Impl::~Impl() {} @@ -190,91 +229,118 @@ void NemoInterface::Impl::start() { void NemoInterface::Impl::stop() { if (running()) { this->_setState(STATE::STOPPED); - this->_connectionTimer.stop(); this->_doAction(); } } -std::future -NemoInterface::Impl::addTiles(const TileArray &tileArray) { +std::shared_future +NemoInterface::Impl::addTiles(const TilePtrArray &tileArray) { using namespace nemo_interface; - if (this->ready()) { + if (tileArray.size() > 0) { + + // copy unknown tiles + auto pTileArray = std::make_shared>>(); + for (const auto *pTile : tileArray) { + auto id = pTile->id(); + const auto it = this->_localTiles.find(id); + Q_ASSERT(it == _localTiles.end()); + if (Q_LIKELY(it == _localTiles.end())) { + auto pTileCopy = + std::make_shared(pTile->coordinateList(), 0.0, id); + _localTiles.insert(std::make_pair(id, pTileCopy)); + pTileArray->push_back(pTileCopy); + } else { + qCDebug(NemoInterfaceLog) + << "addTiles(): tile with id: " << pTile->id() << "already added."; + } + } - // create command. - auto pRosBridge = this->_pRosBridge; - auto pDispatcher = &this->_dispatcher; - Task sendTilesCommand([pRosBridge, tileArray, pDispatcher, - this](std::promise promise) { - // create json object - rapidjson::Document request; - auto &allocator = request.GetAllocator(); - rapidjson::Value jsonTileArray(rapidjson::kArrayType); - for (const auto &tile : tileArray) { - const auto it = _tileMap.find(tile.id()); - if (Q_LIKELY(it == _tileMap.end())) { - - using namespace ros_bridge::messages; - rapidjson::Value jsonTile(rapidjson::kObjectType); - if (!nemo_msgs::tile::toJson(tile, jsonTile, allocator)) { - qCDebug(NemoInterfaceLog) - << "addTiles(): not able to create json object: tile id: " - << tile.id() << " progress: " << tile.progress() - << " points: " << tile.path(); - } - jsonTileArray.PushBack(jsonTile, allocator); - } - } // for - rapidjson::Value tileKey("in_tile_array"); - request.AddMember(tileKey, jsonTileArray, allocator); - - // create response handler. - auto promise_response = std::make_shared>(); - auto future_response = promise_response->get_future(); - auto responseHandler = - [promise_response]( - std::shared_ptr connection, - std::shared_ptr in_message) mutable { - qDebug() << "addTiles: in_message" << in_message->string().c_str(); - promise_response->set_value(); - connection->send_close(1000); - }; - - // call service. - pRosBridge->callService("/nemo/add_tiles", responseHandler, request); - - // wait for response. - auto tStart = hrc::now(); - bool abort = true; - do { - auto status = future_response.wait_for(std::chrono::milliseconds(100)); - if (status == std::future_status::ready) { - abort = false; - break; - } - } while (hrc::now() - tStart < maxResponseTime || - pDispatcher->interruptionPoint()); + // ready for send? + if (pTileArray->size() > 0 && (this->ready() || this->_userSync())) { + + this->_setState(STATE::SYNC_USER); + this->_doAction(); - if (abort) { - qCWarning(NemoInterfaceLog) << "Websocket not responding to request."; - promise.set_value(QVariant(false)); - return; + // create command. + auto sendTilesCommand = std::make_unique( + std::bind(&Impl::_callAddTiles, this, pTileArray)); + + // dispatch command and return. + auto ret = _dispatcher.dispatch(std::move(sendTilesCommand)); + auto sfut = ret.share(); + _futureWatcher.setFuture(sfut); + return sfut; + } + } + + std::promise p; + p.set_value(QVariant(false)); + return p.get_future(); +} + +std::shared_future +NemoInterface::Impl::removeTiles(const IDArray &idArray) { + using namespace nemo_interface; + + if (idArray.size() > 0) { + + // copy known ids + auto pIdArray = std::make_shared(); + for (const auto id : idArray) { + const auto it = this->_localTiles.find(id); + Q_ASSERT(it != _localTiles.end()); + if (Q_LIKELY(it != _localTiles.end())) { + _localTiles.erase(it); + pIdArray->push_back(id); + } else { + qCDebug(NemoInterfaceLog) << "removeTiles(): unknown id: " << id << "."; } + } + + // ready for send? + if (pIdArray->size() > 0 && (this->ready() || this->_userSync())) { + + this->_setState(STATE::SYNC_USER); + this->_doAction(); + + // create command. + auto cmd = std::make_unique( + std::bind(&Impl::_callRemoveTiles, this, pIdArray)); + + // dispatch command and return. + auto ret = _dispatcher.dispatch(std::move(cmd)); + auto sfut = ret.share(); + _futureWatcher.setFuture(sfut); + return sfut; + } + } + + std::promise p; + p.set_value(QVariant(false)); + return p.get_future(); +} - qCritical() << "addTiles(): ToDo: add return value checking here."; +std::shared_future NemoInterface::Impl::clearTiles() { + using namespace nemo_interface; - // update local tiles - QMetaObject::invokeMethod( - this->_parent, std::bind(&Impl::_addTilesLocal, this, tileArray)); + // clear local tiles (_localTiles) + this->_localTiles.clear(); + + if (this->_localTiles.size() > 0 && (this->ready() || this->_userSync())) { + + this->_setState(STATE::SYNC_USER); + this->_doAction(); - // return success - promise.set_value(QVariant(true)); - return; - }); // sendTilesCommand + // create command. + auto pTask = + std::make_unique(std::bind(&Impl::_callClearTiles, this)); // dispatch command and return. - auto ret = _dispatcher.dispatch(sendTilesCommand); - return ret; + auto ret = _dispatcher.dispatch(std::move(pTask)); + auto sfut = ret.share(); + _futureWatcher.setFuture(sfut); + return sfut; } else { std::promise p; p.set_value(QVariant(false)); @@ -286,9 +352,13 @@ TileArray NemoInterface::Impl::getTiles(const IDArray &idArray) { TileArray tileArray; for (const auto &id : idArray) { - const auto it = _tileMap.find(id); - if (it != _tileMap.end()) { - tileArray.append(it->second); + const auto it = _localTiles.find(id); + if (it != _localTiles.end()) { + MeasurementTile copy; + copy.setId(it->second->id()); + copy.setProgress(it->second->progress()); + copy.setPath(it->second->tile()); + tileArray.append(std::move(copy)); } } @@ -298,8 +368,13 @@ TileArray NemoInterface::Impl::getTiles(const IDArray &idArray) { TileArray NemoInterface::Impl::getAllTiles() { TileArray tileArray; - for (const auto &entry : _tileMap) { - tileArray.append(entry.second); + for (const auto &entry : _localTiles) { + auto pTile = entry.second; + MeasurementTile copy; + copy.setId(pTile->id()); + copy.setProgress(pTile->progress()); + copy.setPath(pTile->tile()); + tileArray.append(std::move(copy)); } return tileArray; @@ -309,22 +384,30 @@ LogicalArray NemoInterface::Impl::containsTiles(const IDArray &idArray) { LogicalArray logicalArray; for (const auto &id : idArray) { - const auto &it = _tileMap.find(id); - logicalArray.append(it != _tileMap.end()); + const auto &it = _localTiles.find(id); + logicalArray.append(it != _localTiles.end()); } return logicalArray; } -std::size_t NemoInterface::Impl::size() { return _tileMap.size(); } +std::size_t NemoInterface::Impl::size() { return _localTiles.size(); } -bool NemoInterface::Impl::empty() { return _tileMap.empty(); } +bool NemoInterface::Impl::empty() { return _localTiles.empty(); } ProgressArray NemoInterface::Impl::getProgress() { ProgressArray progressArray; - for (const auto &entry : _tileMap) { - progressArray.append(TaggedProgress{entry.first, entry.second.progress()}); + if (this->_isSynchronized()) { + for (const auto &entry : _remoteTiles) { + progressArray.append( + LabeledProgress{entry.second->progress(), entry.second->id()}); + } + } else { + for (const auto &entry : _localTiles) { + progressArray.append( + LabeledProgress{entry.second->progress(), entry.second->id()}); + } } return progressArray; @@ -333,10 +416,21 @@ ProgressArray NemoInterface::Impl::getProgress() { ProgressArray NemoInterface::Impl::getProgress(const IDArray &idArray) { ProgressArray progressArray; - for (const auto &id : idArray) { - const auto it = _tileMap.find(id); - if (it != _tileMap.end()) { - progressArray.append(TaggedProgress{it->first, it->second.progress()}); + if (this->_isSynchronized()) { + for (const auto &id : idArray) { + const auto it = _remoteTiles.find(id); + if (it != _remoteTiles.end()) { + progressArray.append( + LabeledProgress{it->second->progress(), it->second->id()}); + } + } + } else { + for (const auto &id : idArray) { + const auto it = _localTiles.find(id); + if (it != _localTiles.end()) { + progressArray.append( + LabeledProgress{it->second->progress(), it->second->id()}); + } } } @@ -344,93 +438,76 @@ ProgressArray NemoInterface::Impl::getProgress(const IDArray &idArray) { } NemoInterface::STATUS NemoInterface::Impl::status() { - NemoInterface::STATUS status; - _translate(this->_state, status); - return status; + return _status(this->_state); } bool NemoInterface::Impl::running() { return _running(this->_state); } bool NemoInterface::Impl::ready() { return _ready(this->_state.load()); } -const QString &NemoInterface::Impl::infoString() { return _infoString; } - -const QString &NemoInterface::Impl::warningString() { return _warningString; } +bool NemoInterface::Impl::_sysSync() { return _sysSync(this->_state); } -void NemoInterface::Impl::_addTilesLocal(const TileArray &tileArray) { - bool anyChanges = false; +void NemoInterface::Impl::_onFutureWatcherFinished() { + auto lastTransactionSuccessfull = _futureWatcher.result().toBool(); + if (!lastTransactionSuccessfull) { + if (this->_userSync()) { + _trySynchronize(); + } else if (this->_sysSync()) { + QTimer::singleShot(1000, [this] { this->_trySynchronize(); }); + } + } else { + // fetch progress - for (const auto &tile : tileArray) { - const auto id = tile.id(); - const auto it = _tileMap.find(id); - if (Q_LIKELY(it == _tileMap.end())) { - auto ret = _tileMap.insert(std::make_pair(id, tile)); - anyChanges = true; - Q_ASSERT(ret.second == true); - Q_UNUSED(ret); - } else { - qCDebug(NemoInterfaceLog) - << "_addTilesLocal(): tile with id " << id << " already inserted."; + auto pIdArray = std::make_shared(); + for (const auto &pair : _remoteTiles) { + pIdArray->push_back(pair.first); } - } + auto pTask = std::make_unique( + std::bind(&Impl::_callGetProgress, this, pIdArray)); - if (anyChanges) { - emit _parent->tilesChanged(); + // dispatch command. + auto ret = _dispatcher.dispatch(std::move(pTask)); + Q_ASSERT(false); + _futureWatcher.setFuture(ret.share()); } } -void NemoInterface::Impl::_removeTilesLocal(const IDArray &idArray) { - bool anyChanges = false; - for (const auto &id : idArray) { - const auto it = _tileMap.find(id); - if (Q_LIKELY(it != _tileMap.end())) { - _tileMap.erase(it); - anyChanges = true; - } else { - qCDebug(NemoInterfaceLog) - << "_removeTilesLocal(): tile with id " << id << " not found."; - } - } +bool NemoInterface::Impl::_userSync() { return _userSync(this->_state); } - if (anyChanges) { - emit _parent->tilesChanged(); - } -} +const QString &NemoInterface::Impl::infoString() { return _infoString; } -void NemoInterface::Impl::_clearTilesLocal() { - if (!_tileMap.empty()) { - _tileMap.clear(); - emit _parent->tilesChanged(); - } -} +const QString &NemoInterface::Impl::warningString() { return _warningString; } -void NemoInterface::Impl::_updateProgress(ProgressArray progressArray) { - for (auto itPair = progressArray.begin(); itPair != progressArray.end();) { +void NemoInterface::Impl::_updateProgress( + std::shared_ptr pArray) { + for (auto itLP = pArray->begin(); itLP != pArray->end();) { - const auto &id = itPair->first; - auto it = _tileMap.find(id); + auto it = _remoteTiles.find(itLP->id()); - if (Q_LIKELY(it != _tileMap.end())) { - const auto &progress = itPair->second; - it->second.setProgress(progress); - ++itPair; + if (Q_LIKELY(it != _remoteTiles.end())) { + it->second->setProgress(itLP->progress()); + ++itLP; } else { qCDebug(NemoInterfaceLog) - << "_updateProgress(): tile with id " << id << " not found."; - itPair = progressArray.erase(itPair); + << "_updateProgress(): tile with id " << itLP->id() << " not found."; + itLP = pArray->erase(itLP); } } - emit _parent->progressChanged(progressArray); + if (pArray->size() > 0) { + emit _parent->progressChanged(*pArray); + } } -void NemoInterface::Impl::_setHeartbeat(const QNemoHeartbeat &hb) { - if (this->_lastHeartbeat != hb) { - _lastHeartbeat = hb; - if (ready()) { - this->_timeoutTimer.stop(); - this->_timeoutTimer.start(NO_HEARTBEAT_TIMEOUT); - } +void NemoInterface::Impl::_onHeartbeatReceived(const QNemoHeartbeat &hb) { + _lastHeartbeat = hb; + this->_timeoutTimer.start(NO_HEARTBEAT_TIMEOUT); + if (this->_state == STATE::TRY_TOPIC_SERVICE_SETUP) { + this->_setState(STATE::READY); + this->_doAction(); + } else if (this->_state == STATE::HEARTBEAT_TIMEOUT) { + this->_setState(STATE::READY); + this->_doAction(); } } @@ -456,57 +533,611 @@ void NemoInterface::Impl::_doTopicServiceSetup() { this->_pRosBridge->addClient(progressClient); this->_pRosBridge->subscribe( progressClient, "/nemo/progress", - [this](std::shared_ptr connection, + [this](std::shared_ptr, std::shared_ptr in_message) { - qDebug() << "doTopicServiceSetup(): /nemo/progress: " - << in_message->string().c_str(); - qDebug() << "impl missing"; + auto msg = in_message->string(); + + // parse in_message + rapidjson::Document d; + d.Parse(msg.c_str()); + if (!d.HasParseError()) { + if (d.HasMember("msg") && d["msg"].IsObject()) { + + // create obj from json + nemo_msgs::progress_array::ProgressArray progressArray; + if (nemo_msgs::progress_array::fromJson(d["msg"], progressArray)) { + + // correct range errors of progress + for (auto &lp : progressArray.progress_array()) { + bool rangeError = false; + if (lp.progress() < 0) { + lp.setProgress(0); + rangeError = true; + } + if (lp.progress() > 100) { + lp.setProgress(100); + rangeError = true; + } + + if (rangeError) { + qCWarning(NemoInterfaceLog) << "/nemo/progress progress out " + "of range, value was set to: " + << lp.progress(); + } + } + + auto p = std::make_shared(); + *p = std::move(progressArray.progress_array()); + QMetaObject::invokeMethod( + this->_parent, + std::bind(&NemoInterface::Impl::_updateProgress, this, p)); + } else { + qCWarning(NemoInterfaceLog) << "/nemo/progress not able to " + "create ProgressArray form json: " + << msg.c_str(); + } + } else { + qCWarning(NemoInterfaceLog) + << "/nemo/progress no \"msg\" key or wrong type: " + << msg.c_str(); + } + } else { + qCWarning(NemoInterfaceLog) + << "/nemo/progress message parse error (" << d.GetParseError() + << "): " << msg.c_str(); + } }); // Subscribe heartbeat msg. const char *heartbeatClient = "client:/nemo/heartbeat"; this->_pRosBridge->addClient(heartbeatClient); this->_pRosBridge->subscribe( heartbeatClient, "/nemo/heartbeat", - [this](std::shared_ptr connection, + [this](std::shared_ptr, std::shared_ptr in_message) { - qDebug() << "doTopicServiceSetup(): /nemo/heartbeat: " - << in_message->string().c_str(); - qDebug() << "impl missing"; + auto msg = in_message->string(); + + // parse in_message + rapidjson::Document d; + d.Parse(msg.c_str()); + if (!d.HasParseError()) { + if (d.HasMember("msg") && d["msg"].IsObject()) { + + // create obj from json + nemo_msgs::heartbeat::Heartbeat heartbeat; + if (nemo_msgs::heartbeat::fromJson(d["msg"], heartbeat)) { + QMetaObject::invokeMethod( + this->_parent, + std::bind(&NemoInterface::Impl::_onHeartbeatReceived, this, + heartbeat)); + } else { + qCWarning(NemoInterfaceLog) << "/nemo/heartbeat not able to " + "create Heartbeat form json: " + << msg.c_str(); + } + } else { + qCWarning(NemoInterfaceLog) + << "/nemo/heartbeat no \"msg\" key or wrong type: " + << msg.c_str(); + } + } else { + qCWarning(NemoInterfaceLog) + << "/nemo/heartbeat message parse error (" << d.GetParseError() + << "): " << msg.c_str(); + } }); } +void NemoInterface::Impl::_trySynchronize() { + if (!_isSynchronized()) { + this->_setState(STATE::SYNC_SYS); + this->_doAction(); + + // create clear cmd. + auto pTask = std::make_unique( + std::bind(&Impl::_callClearTiles, this)); + + // dispatch command. + qCritical() << "this assert is triggered sometimes! sdf92894"; + Q_ASSERT(_dispatcher.pendingTasks() == 0); + auto ret = _dispatcher.dispatch(std::move(pTask)); + + // create tile array. + auto pTileArray = std::make_shared>>(); + for (auto pair : _localTiles) { + pTileArray->push_back(pair.second); + } + + // create addTiles cmd. + auto sendTilesCommand = std::make_unique( + std::bind(&Impl::_callAddTiles, this, pTileArray)); + + // dispatch command. + ret = _dispatcher.dispatch(std::move(sendTilesCommand)); + _futureWatcher.setFuture(ret.share()); + } +} + +bool NemoInterface::Impl::_isSynchronized() { + return _localTiles.size() > 0 && _remoteTiles.size() > 0 && + std::equal( + _localTiles.begin(), _localTiles.end(), _remoteTiles.begin(), + [](const auto &a, const auto &b) { return a.first == b.first; }); +} + void NemoInterface::Impl::_doAction() { - // Check ROS Bridge status and do setup if necessary. + static bool resetDone = false; switch (this->_state) { case STATE::STOPPED: + this->_connectionTimer.stop(); + this->_timeoutTimer.stop(); + this->_clearTilesRemote(); if (this->_pRosBridge->running()) { this->_pRosBridge->reset(); } break; case STATE::START_BRIDGE: - case STATE::TIMEOUT: - this->_pRosBridge->reset(); this->_pRosBridge->run(); - this->_connectionTimer.start(EVENT_TIMER_INTERVAL); + this->_connectionTimer.start(CONNECTION_TIMER_INTERVAL); + break; break; case STATE::WEBSOCKET_DETECTED: + resetDone = false; this->_setState(STATE::TRY_TOPIC_SERVICE_SETUP); this->_doAction(); break; case STATE::TRY_TOPIC_SERVICE_SETUP: this->_doTopicServiceSetup(); - this->_setState(STATE::READY); + this->_timeoutTimer.start(NO_HEARTBEAT_TIMEOUT); break; case STATE::READY: + _trySynchronize(); + break; + case STATE::SYNC_USER: + case STATE::SYNC_SYS: + break; + case STATE::HEARTBEAT_TIMEOUT: + this->_clearTilesRemote(); + break; + case STATE::WEBSOCKET_TIMEOUT: + if (!resetDone) { + resetDone = true; + this->_pRosBridge->reset(); + this->_pRosBridge->run(); + } + this->_clearTilesRemote(); break; }; } -bool NemoInterface::Impl::_setState(STATE s) { - if (s != this->_state) { - this->_state = s; - emit this->_parent->statusChanged(); +QVariant NemoInterface::Impl::_callAddTiles( + + std::shared_ptr>> pTileArray) { + // create json object + rapidjson::Document request(rapidjson::kObjectType); + auto &allocator = request.GetAllocator(); + rapidjson::Value jsonTileArray(rapidjson::kArrayType); + for (const auto &tile : *pTileArray) { + using namespace ros_bridge::messages; + rapidjson::Value jsonTile(rapidjson::kObjectType); + if (!nemo_msgs::tile::toJson(*tile, jsonTile, allocator)) { + qCDebug(NemoInterfaceLog) + << "addTiles(): not able to create json object: tile id: " + << tile->id() << " progress: " << tile->progress() + << " points: " << tile->tile(); + } + jsonTileArray.PushBack(jsonTile, allocator); + } // for + rapidjson::Value tileKey("in_tile_array"); + request.AddMember(tileKey, jsonTileArray, allocator); + + // create response handler. + auto promise_response = std::make_shared>(); + auto future_response = promise_response->get_future(); + auto responseHandler = + [promise_response]( + std::shared_ptr connection, + std::shared_ptr in_message) mutable { + // check if transaction was successfull + auto msg = in_message->string(); + rapidjson::Document d; + d.Parse(msg.c_str()); + if (!d.HasParseError()) { + if (d.HasMember("values") && d["values"].IsObject()) { + auto values = d["values"].GetObject(); + if (values.HasMember("success") && values["success"].IsBool()) { + promise_response->set_value(values["success"].GetBool()); + } else { + qCWarning(NemoInterfaceLog) + << "/nemo/add_tiles no \"success\" key or wrong type: " + << msg.c_str(); + promise_response->set_value(false); + } + } else { + qCWarning(NemoInterfaceLog) + << "/nemo/add_tiles no \"values\" key or wrong type: " + << msg.c_str(); + promise_response->set_value(false); + } + } else { + qCWarning(NemoInterfaceLog) + << "/nemo/add_tiles message parse error (" << d.GetParseError() + << "): " << msg.c_str(); + promise_response->set_value(false); + } + connection->send_close(1000); + }; + + // call service. + this->_pRosBridge->callService("/nemo/add_tiles", responseHandler, request); + + // wait for response. + auto tStart = hrc::now(); + bool abort = true; + do { + auto status = future_response.wait_for(std::chrono::milliseconds(100)); + if (status == std::future_status::ready) { + abort = false; + break; + } + } while (hrc::now() - tStart < maxResponseTime || + this->_dispatcher.isInterruptionRequested()); + + if (abort) { + qCWarning(NemoInterfaceLog) + << "addTiles(): Websocket not responding to request."; + return QVariant(false); + } + + // transaction error? + if (!future_response.get()) { + return QVariant(false); + } + + // add remote tiles (_remoteTiles) + QMetaObject::invokeMethod(this->_parent /* context */, [this, pTileArray] { + this->_addTilesRemote(pTileArray); + }); + + // return success + return QVariant(true); +} + +QVariant +NemoInterface::Impl::_callRemoveTiles(std::shared_ptr pIdArray) { + // create json object + rapidjson::Document request(rapidjson::kObjectType); + auto &allocator = request.GetAllocator(); + rapidjson::Value jsonIdArray(rapidjson::kArrayType); + for (const auto id : *pIdArray) { + using namespace ros_bridge::messages; + jsonIdArray.PushBack(rapidjson::Value(id), allocator); + } // for + rapidjson::Value tileKey("ids"); + request.AddMember(tileKey, jsonIdArray, allocator); + + // create response handler. + auto promise_response = std::make_shared>(); + auto future_response = promise_response->get_future(); + auto responseHandler = + [promise_response]( + std::shared_ptr connection, + std::shared_ptr in_message) mutable { + // check if transaction was successfull + auto msg = in_message->string(); + rapidjson::Document d; + d.Parse(msg.c_str()); + if (!d.HasParseError()) { + if (d.HasMember("values") && d["values"].IsObject()) { + auto values = d["values"].GetObject(); + if (values.HasMember("success") && values["success"].IsBool()) { + promise_response->set_value(values["success"].GetBool()); + } else { + qCWarning(NemoInterfaceLog) + << "/nemo/remove_tiles no \"success\" key or wrong type: " + << msg.c_str(); + promise_response->set_value(false); + } + } else { + qCWarning(NemoInterfaceLog) + << "/nemo/remove_tiles no \"values\" key or wrong type: " + << msg.c_str(); + promise_response->set_value(false); + } + } else { + qCWarning(NemoInterfaceLog) + << "/nemo/remove_tiles message parse error (" << d.GetParseError() + << "): " << msg.c_str(); + promise_response->set_value(false); + } + connection->send_close(1000); + }; + + // call service. + this->_pRosBridge->callService("/nemo/remove_tiles", responseHandler, + request); + + // wait for response. + auto tStart = hrc::now(); + bool abort = true; + do { + auto status = future_response.wait_for(std::chrono::milliseconds(100)); + if (status == std::future_status::ready) { + abort = false; + break; + } + } while (hrc::now() - tStart < maxResponseTime || + this->_dispatcher.isInterruptionRequested()); + + if (abort) { + qCWarning(NemoInterfaceLog) + << "remove_tiles(): Websocket not responding to request."; + return QVariant(false); + } + + // transaction error? + if (!future_response.get()) { + return QVariant(false); + } + + // remove remote tiles (_remoteTiles) + QMetaObject::invokeMethod(this->_parent /* context */, [this, pIdArray] { + this->_removeTilesRemote(pIdArray); + }); + + // return success + return QVariant(true); +} + +QVariant NemoInterface::Impl::_callClearTiles() { + // create response handler. + auto promise_response = std::make_shared>(); + auto future_response = promise_response->get_future(); + auto responseHandler = + [promise_response]( + std::shared_ptr connection, + std::shared_ptr in_message) mutable { + // check if transaction was successfull + auto msg = in_message->string(); + rapidjson::Document d; + d.Parse(msg.c_str()); + if (!d.HasParseError()) { + if (d.HasMember("result") && d["result"].IsBool()) { + promise_response->set_value(d["result"].GetBool()); + } else { + qCWarning(NemoInterfaceLog) + << "/nemo/clear_tiles no \"result\" key or wrong type: " + << msg.c_str(); + promise_response->set_value(false); + } + } else { + qCWarning(NemoInterfaceLog) + << "/nemo/clear_tiles message parse error (" << d.GetParseError() + << "): " << msg.c_str(); + promise_response->set_value(false); + } + connection->send_close(1000); + }; + + // call service. + this->_pRosBridge->callService("/nemo/clear_tiles", responseHandler, + rapidjson::Document(rapidjson::kObjectType)); + + // wait for response. + auto tStart = hrc::now(); + bool abort = true; + do { + auto status = future_response.wait_for(std::chrono::milliseconds(100)); + if (status == std::future_status::ready) { + abort = false; + break; + } + } while (hrc::now() - tStart < maxResponseTime || + this->_dispatcher.isInterruptionRequested()); + + if (abort) { + qCWarning(NemoInterfaceLog) << "Websocket not responding to request."; + return QVariant(false); + } + + // transaction failed? + if (!future_response.get()) { + return QVariant(false); + } + + // clear remote tiles (_remoteTiles) + QMetaObject::invokeMethod(this->_parent, + std::bind(&Impl::_clearTilesRemote, this)); + + // return success + return QVariant(true); +} + +QVariant +NemoInterface::Impl::_callGetProgress(std::shared_ptr pIdArray) { + // create json object + rapidjson::Document request(rapidjson::kObjectType); + auto &allocator = request.GetAllocator(); + rapidjson::Value jsonIdArray(rapidjson::kArrayType); + for (const auto id : *pIdArray) { + using namespace ros_bridge::messages; + jsonIdArray.PushBack(rapidjson::Value(id), allocator); + } // for + rapidjson::Value tileKey("ids"); + request.AddMember(tileKey, jsonIdArray, allocator); + + // create response handler. + typedef std::shared_ptr ResponseType; + auto promise_response = std::make_shared>(); + auto future_response = promise_response->get_future(); + auto responseHandler = + [promise_response]( + std::shared_ptr connection, + std::shared_ptr in_message) mutable { + // check if transaction was successfull + auto msg = in_message->string(); + rapidjson::Document d; + d.Parse(msg.c_str()); + if (!d.HasParseError()) { + if (d.HasMember("values") && d["values"].IsObject()) { + auto values = d["values"].GetObject(); + ros_bridge::messages::nemo_msgs::progress_array::ProgressArray + progressArrayMsg; + if (ros_bridge::messages::nemo_msgs::progress_array::fromJson( + d["values"], progressArrayMsg)) { + auto pArray = std::make_shared(); + *pArray = std::move(progressArrayMsg.progress_array()); + promise_response->set_value(pArray); + } else { + qCWarning(NemoInterfaceLog) + << "/nemo/get_progress error while creating ProgressArray " + "from json."; + promise_response->set_value(nullptr); + } + } else { + qCWarning(NemoInterfaceLog) + << "/nemo/get_progress no \"values\" key or wrong type: " + << msg.c_str(); + promise_response->set_value(nullptr); + } + } else { + qCWarning(NemoInterfaceLog) + << "/nemo/get_progress message parse error (" << d.GetParseError() + << "): " << msg.c_str(); + promise_response->set_value(nullptr); + } + connection->send_close(1000); + }; + + // call service. + this->_pRosBridge->callService("/nemo/get_progress", responseHandler, + request); + + // wait for response. + auto tStart = hrc::now(); + bool abort = true; + do { + auto status = future_response.wait_for(std::chrono::milliseconds(100)); + if (status == std::future_status::ready) { + abort = false; + break; + } + } while (hrc::now() - tStart < maxResponseTime || + this->_dispatcher.isInterruptionRequested()); + + if (abort) { + qCWarning(NemoInterfaceLog) + << "remove_tiles(): Websocket not responding to request."; + return QVariant(false); + } + + // transaction error? + auto pArray = future_response.get(); + if (pArray == nullptr) { + return QVariant(false); + } + + // remove remote tiles (_remoteTiles) + QMetaObject::invokeMethod(this->_parent /* context */, + [this, pArray] { this->_updateProgress(pArray); }); + + // return success + return QVariant(true); +} + +void NemoInterface::Impl::_addTilesRemote( + std::shared_ptr>> pTileArray) { + auto pArrayDup = std::make_shared>>(); + for (auto pTile : *pTileArray) { + pArrayDup->push_back(std::make_shared(*pTile)); + } + _addTilesRemote(pArrayDup); +} + +void NemoInterface::Impl::_addTilesRemote( + std::shared_ptr>> pTileArray) { + + bool anyChange = false; + + for (auto pTile : *pTileArray) { + auto id = pTile->id(); + auto it = _remoteTiles.find(id); + qCritical() << "this assert is triggered sometimes! 1212341242"; + Q_ASSERT(it == _remoteTiles.end()); + if (Q_LIKELY(it == _remoteTiles.end())) { + auto ret = _remoteTiles.insert(std::make_pair(id, pTile)); + Q_ASSERT(ret.second == true); + Q_UNUSED(ret); + anyChange = true; + } else { + qCWarning(NemoInterfaceLog) + << "_addTilesRemote: tile with id " << id << " already added."; + } + } + + if (anyChange) { + if (this->_isSynchronized()) { + this->_setState(STATE::READY); + this->_doAction(); + } + this->_parent->tilesChanged(); + } +} + +void NemoInterface::Impl::_removeTilesRemote(std::shared_ptr idArray) { + bool anyChange = false; + + for (const auto id : *idArray) { + auto it = _remoteTiles.find(id); + Q_ASSERT(it != _remoteTiles.end()); + if (Q_LIKELY(it != _remoteTiles.end())) { + _remoteTiles.erase(it); + anyChange = true; + } else { + qCWarning(NemoInterfaceLog) + << "_removeTilesRemote: tile with unknown id " << id << "."; + } + } + + if (anyChange) { + if (this->_isSynchronized()) { + this->_setState(STATE::READY); + this->_doAction(); + } + this->_parent->tilesChanged(); + } +} + +void NemoInterface::Impl::_clearTilesRemote() { + if (_remoteTiles.size() > 0) { + _remoteTiles.clear(); + if (this->_isSynchronized()) { + this->_setState(STATE::READY); + this->_doAction(); + } + this->_parent->tilesChanged(); + } +} + +bool NemoInterface::Impl::_setState(STATE newState) { + if (newState != this->_state) { + auto oldState = this->_state.load(); + this->_state = newState; + + qCDebug(NemoInterfaceLog) + << "state: " << _toString(oldState) << " -> " << _toString(newState); + auto oldStatus = _status(oldState); + auto newStatus = _status(newState); + if (oldStatus != newStatus) { + qCDebug(NemoInterfaceLog) << "status: " << _toString(oldStatus) << " -> " + << _toString(newStatus); + emit this->_parent->statusChanged(); + } + + if (_running(oldState) != _running(newState)) { + emit this->_parent->runningChanged(); + } + return true; } else { return false; @@ -517,10 +1148,90 @@ bool NemoInterface::Impl::_ready(NemoInterface::Impl::STATE s) { return s == STATE::READY; } +bool NemoInterface::Impl::_userSync(NemoInterface::Impl::STATE s) { + return s == STATE::SYNC_USER; +} + +bool NemoInterface::Impl::_sysSync(NemoInterface::Impl::STATE s) { + return s == STATE::SYNC_SYS; +} + bool NemoInterface::Impl::_running(NemoInterface::Impl::STATE s) { return s != STATE::STOPPED; } +NemoInterface::STATUS +NemoInterface::Impl::_status(NemoInterface::Impl::STATE state) { + NemoInterface::STATUS status; + switch (state) { + case STATE::STOPPED: + status = NemoInterface::STATUS::NOT_CONNECTED; + break; + case STATE::START_BRIDGE: + status = NemoInterface::STATUS::NOT_CONNECTED; + break; + case STATE::WEBSOCKET_DETECTED: + status = NemoInterface::STATUS::WEBSOCKET_DETECTED; + break; + case STATE::TRY_TOPIC_SERVICE_SETUP: + status = NemoInterface::STATUS::WEBSOCKET_DETECTED; + break; + case STATE::READY: + status = NemoInterface::STATUS::READY; + break; + case STATE::SYNC_USER: + case STATE::SYNC_SYS: + status = NemoInterface::STATUS::SYNC; + break; + case STATE::WEBSOCKET_TIMEOUT: + case STATE::HEARTBEAT_TIMEOUT: + status = NemoInterface::STATUS::TIMEOUT; + break; + } + + return status; +} + +QString NemoInterface::Impl::_toString(NemoInterface::Impl::STATE s) { + switch (s) { + case STATE::STOPPED: + return QString("STOPPED"); + case STATE::START_BRIDGE: + return QString("START_BRIDGE"); + case STATE::WEBSOCKET_DETECTED: + return QString("WEBSOCKET_DETECTED"); + case STATE::TRY_TOPIC_SERVICE_SETUP: + return QString("TRY_TOPIC_SERVICE_SETUP"); + case STATE::READY: + return QString("READY"); + case STATE::SYNC_USER: + return QString("SYNC_USER"); + case STATE::SYNC_SYS: + return QString("SYNC_SYS"); + case STATE::WEBSOCKET_TIMEOUT: + return QString("WEBSOCKET_TIMEOUT"); + case STATE::HEARTBEAT_TIMEOUT: + return QString("HEARTBEAT_TIMEOUT"); + } + return "unknown state!"; +} + +QString NemoInterface::Impl::_toString(NemoInterface::STATUS s) { + switch (s) { + case NemoInterface::STATUS::NOT_CONNECTED: + return QString("NOT_CONNECTED"); + case NemoInterface::STATUS::READY: + return QString("READY"); + case NemoInterface::STATUS::TIMEOUT: + return QString("TIMEOUT"); + case NemoInterface::STATUS::WEBSOCKET_DETECTED: + return QString("WEBSOCKET_DETECTED"); + case NemoInterface::STATUS::SYNC: + return QString("SYNC"); + } + return "unknown state!"; +} + // =============================================================== // NemoInterface NemoInterface::NemoInterface() @@ -539,15 +1250,28 @@ void NemoInterface::start() { this->pImpl->start(); } void NemoInterface::stop() { this->pImpl->stop(); } -void NemoInterface::addTiles(const TileArray &tileArray) { - this->pImpl->addTiles(tileArray); +std::shared_future +NemoInterface::addTiles(const TileArray &tileArray) { + TilePtrArray ptrArray; + for (const auto &tile : tileArray) { + ptrArray.push_back(const_cast(&tile)); + } + return this->pImpl->addTiles(ptrArray); } -void NemoInterface::removeTiles(const IDArray &idArray) { - this->pImpl->removeTiles(idArray); +std::shared_future +NemoInterface::addTiles(const TilePtrArray &tileArray) { + return this->pImpl->addTiles(tileArray); } -void NemoInterface::clearTiles() { this->pImpl->clearTiles(); } +std::shared_future +NemoInterface::removeTiles(const IDArray &idArray) { + return this->pImpl->removeTiles(idArray); +} + +std::shared_future NemoInterface::clearTiles() { + return this->pImpl->clearTiles(); +} TileArray NemoInterface::getTiles(const IDArray &idArray) { return this->pImpl->getTiles(idArray); diff --git a/src/MeasurementComplexItem/NemoInterface.h b/src/MeasurementComplexItem/NemoInterface.h index bfd84c07c..7b221cc49 100644 --- a/src/MeasurementComplexItem/NemoInterface.h +++ b/src/MeasurementComplexItem/NemoInterface.h @@ -2,7 +2,9 @@ #include #include +#include +#include #include #include "IDArray.h" @@ -28,10 +30,10 @@ public: enum class STATUS { NOT_CONNECTED, + SYNC, READY, WEBSOCKET_DETECTED, TIMEOUT, - INVALID_HEARTBEAT }; Q_ENUM(STATUS) @@ -49,9 +51,28 @@ public: Q_INVOKABLE void stop(); // Tile editing. - void addTiles(const TileArray &tileArray); - void removeTiles(const IDArray &idArray); - void clearTiles(); + //! + //! \brief addTiles + //! \param tileArray + //! \return Returns a QVariant containing a boolean value, indicating if the + //! requested operation was successfull. + //! + std::shared_future addTiles(const TileArray &tileArray); + std::shared_future addTiles(const TilePtrArray &tileArray); + //! + //! \brief removeTiles + //! \param idArray + //! \return Returns a QVariant containing a boolean value, indicating if the + //! requested operation was successfull. + //! + std::shared_future removeTiles(const IDArray &idArray); + //! + //! \brief clearTiles + //! \return Returns a QVariant containing a boolean value, indicating if the + //! requested operation was successfull. + //! + std::shared_future clearTiles(); + TileArray getTiles(const IDArray &idArray); TileArray getAllTiles(); LogicalArray containsTiles(const IDArray &idArray); diff --git a/src/MeasurementComplexItem/geometry/GeoArea.cc b/src/MeasurementComplexItem/geometry/GeoArea.cc index a385b7444..02963b760 100644 --- a/src/MeasurementComplexItem/geometry/GeoArea.cc +++ b/src/MeasurementComplexItem/geometry/GeoArea.cc @@ -82,20 +82,7 @@ bool GeoArea::covers(const QGeoCoordinate &c) { } } -void GeoArea::init() { - this->setObjectName(nameString); - // connect(this, &GeoArea::pathChanged, [this] { - // if (this->objectName() != "Tile") { - // qDebug() << this->objectName() << " path: " << this->path() << "\n"; - // } - // }); - // connect(this, &GeoArea::centerChanged, [this] { - // if (this->objectName() != "Tile") { - // qDebug() << this->objectName() << " center: " << this->center() << - // "\n"; - // } - // }); -} +void GeoArea::init() { this->setObjectName(nameString); } void GeoArea::setErrorString(const QString &str) { this->_errorString = str; diff --git a/src/MeasurementComplexItem/geometry/MeasurementArea.cc b/src/MeasurementComplexItem/geometry/MeasurementArea.cc index 763f8b33e..e554a09b9 100644 --- a/src/MeasurementComplexItem/geometry/MeasurementArea.cc +++ b/src/MeasurementComplexItem/geometry/MeasurementArea.cc @@ -33,23 +33,6 @@ namespace { const char *tileArrayKey = "TileArray"; } // namespace -TileData::TileData() {} -TileData::~TileData() {} - -TileData &TileData::operator=(const TileData &other) { return *this; } -bool TileData::operator==(const TileData &other) const { return false; } -bool TileData::operator!=(const TileData &other) const { - return operator==(other); -} - -void TileData::saveToJson(QJsonObject &json) {} -bool TileData::loadFromJson(const QJsonObject &json, QString &errorString) { - return false; -} - -void TileData::clear() {} -std::size_t TileData::size() const { return 0; } - const char *MeasurementArea::settingsGroup = "MeasurementArea"; const char *tileHeightKey = "TileHeight"; const char *tileWidthName = "TileWidth"; @@ -107,7 +90,7 @@ MeasurementArea::MeasurementArea(const MeasurementArea &other, QObject *parent) qobject_cast(other._tiles->operator[](i)) ->clone(_tiles.get())); } - _tileMap = other._tileMap; + _indexMap = other._indexMap; enableUpdate(); } else { enableUpdate(); @@ -131,7 +114,7 @@ MeasurementArea &MeasurementArea::operator=(const MeasurementArea &other) { qobject_cast(other._tiles->operator[](i)) ->clone(_tiles.get())); } - _tileMap = other._tileMap; + _indexMap = other._indexMap; enableUpdate(); } else { enableUpdate(); @@ -270,6 +253,41 @@ bool MeasurementArea::loadFromJson(const QJsonObject &json, break; } } + + if (!tileError) { + this->_indexMap.clear(); + for (int i = 0; i < _tiles->count(); ++i) { + + auto tile = qobject_cast(_tiles->get(i)); + auto it = _indexMap.find(tile->id()); + + // find unique id + if (it != _indexMap.end()) { + auto newId = tile->id() + 1; + constexpr long counterMax = 1e6; + unsigned long counter = 0; + for (; counter <= counterMax; ++counter) { + it = _indexMap.find(newId); + if (it == _indexMap.end()) { + break; + } else { + ++newId; + } + } + + if (counter != counterMax) { + tile->setId(newId); + tile->setProgress(0.0); + } else { + qCritical() << "MeasurementArea::storeTiles(): not able to find " + "unique id!"; + continue; + } + } + + _indexMap.insert(std::make_pair(tile->id(), i)); + } + } } else { qCWarning(MeasurementAreaLog) << "Not able to load tiles. tileArrayKey missing or wrong type."; @@ -307,15 +325,22 @@ bool MeasurementArea::isCorrect() { void MeasurementArea::updateProgress(const ProgressArray &array) { if (ready() && !_holdProgress && array.size() > 0) { bool anyChanges = false; - for (const auto &pair : array) { - const auto &id = pair.first; - const auto &progress = pair.second; - auto it = _tileMap.find(id); - if (it != _tileMap.end()) { - if (!qFuzzyCompare(progress, it->second->progress())) { - it->second->setProgress(progress); + long counter = 0; + for (const auto &lp : array) { + qDebug() << "MeasurementArea::updateProgress: counter = " << counter++; + + auto it = _indexMap.find(lp.id()); + if (it != _indexMap.end()) { + int tileIndex = it->second; + auto *tile = _tiles->value(tileIndex); + qDebug() << "MeasurementArea::updateProgress: progress before = " + << tile->progress(); + if (!qFuzzyCompare(lp.progress(), tile->progress())) { + tile->setProgress(lp.progress()); anyChanges = true; } + qDebug() << "MeasurementArea::updateProgress: progress after = " + << tile->progress(); } } @@ -330,6 +355,8 @@ void MeasurementArea::randomProgress() { std::srand(std::time(nullptr)); + ProgressArray progressArray; + for (int i = 0; i < _tiles->count(); ++i) { auto tile = _tiles->value(i); @@ -341,10 +368,10 @@ void MeasurementArea::randomProgress() { p = 100; } - tile->setProgress(p); + progressArray.append(LabeledProgress(p, tile->id())); } - emit progressChanged(); + updateProgress(progressArray); } } @@ -421,7 +448,7 @@ void MeasurementArea::doUpdate() { geoTile->push_back(geoVertex); hashValue ^= hashFun(geoVertex); } - geoTile->setId(long(hashValue)); + geoTile->setId(std::int64_t(hashValue)); pData->append(geoTile); } } @@ -452,7 +479,7 @@ void MeasurementArea::deferUpdate() { if (this->_state == STATE::IDLE || this->_state == STATE::DEFERED) { qCDebug(MeasurementAreaLog) << "defereUpdate(): defer update."; if (this->_state == STATE::IDLE) { - this->_tileMap.clear(); + this->_indexMap.clear(); this->_tiles->clearAndDeleteContents(); emit tilesChanged(); emit progressChanged(); @@ -478,20 +505,20 @@ void MeasurementArea::storeTiles() { QQmlEngine::CppOwnership); // update tileMap - this->_tileMap.clear(); + this->_indexMap.clear(); for (int i = 0; i < _tiles->count(); ++i) { auto tile = qobject_cast(_tiles->get(i)); - auto it = _tileMap.find(tile->id()); + auto it = _indexMap.find(tile->id()); // find unique id - if (it != _tileMap.end()) { - long newId = tile->id() + 1; + if (it != _indexMap.end()) { + auto newId = tile->id() + 1; constexpr long counterMax = 1e6; unsigned long counter = 0; for (; counter <= counterMax; ++counter) { - it = _tileMap.find(newId); - if (it == _tileMap.end()) { + it = _indexMap.find(newId); + if (it == _indexMap.end()) { break; } else { ++newId; @@ -508,7 +535,7 @@ void MeasurementArea::storeTiles() { } } - _tileMap.insert(std::make_pair(tile->id(), tile)); + _indexMap.insert(std::make_pair(tile->id(), i)); } // This is expensive. Drawing tiles is expensive too. @@ -581,18 +608,20 @@ void MeasurementArea::setHoldProgress(bool holdProgress) { void MeasurementArea::updateIds(const QList &array) { for (const auto &diff : array) { - auto it = _tileMap.find(diff.oldTile.id()); - - if (it != _tileMap.end() && - diff.oldTile.coordinateList() == it->second->coordinateList()) { - // Change id and update _tileMap. - const auto newId = diff.newTile.id(); - auto tile = it->second; - tile->setId(newId); - _tileMap.erase(it); - auto ret = _tileMap.insert(std::make_pair(newId, tile)); - Q_ASSERT(ret.second == true /*insert success?*/); - Q_UNUSED(ret); + auto it = _indexMap.find(diff.oldTile.id()); + + if (it != _indexMap.end()) { + int tileIndex = it->second; + auto *tile = _tiles->value(tileIndex); + if (diff.oldTile.coordinateList() == tile->coordinateList()) { + // Change id and update _tileMap. + const auto newId = diff.newTile.id(); + tile->setId(newId); + _indexMap.erase(it); + auto ret = _indexMap.insert(std::make_pair(newId, tileIndex)); + Q_ASSERT(ret.second == true /*insert success?*/); + Q_UNUSED(ret); + } } } } @@ -673,7 +702,7 @@ bool getTiles(const FPolygon &area, Length tileHeight, Length tileWidth, boost_tiles)) continue; - for (FPolygon t : boost_tiles) { + for (FPolygon &t : boost_tiles) { if (bg::area(t) > minTileArea.value()) { // Transform boost_tile to world coordinate system. FPolygon rotated_tile; diff --git a/src/MeasurementComplexItem/geometry/MeasurementArea.h b/src/MeasurementComplexItem/geometry/MeasurementArea.h index d86b49e17..0f17c9c0b 100644 --- a/src/MeasurementComplexItem/geometry/MeasurementArea.h +++ b/src/MeasurementComplexItem/geometry/MeasurementArea.h @@ -54,10 +54,10 @@ public: // Overrides from GeoArea QString mapVisualQML(void) const override; QString editorQML(void) const override; - MeasurementArea *clone(QObject *parent = nullptr) const; + MeasurementArea *clone(QObject *parent = nullptr) const override; bool saveToJson(QJsonObject &json) override; bool loadFromJson(const QJsonObject &json, QString &errorString) override; - Q_INVOKABLE virtual bool isCorrect(); + Q_INVOKABLE virtual bool isCorrect() override; // Property getters. Fact *tileHeight(); @@ -122,7 +122,7 @@ private: // Tile stuff. TilePtr _tiles; - std::map _tileMap; + std::map _indexMap; bool _holdProgress; QTimer _timer; STATE _state; diff --git a/src/MeasurementComplexItem/geometry/ProgressArray.h b/src/MeasurementComplexItem/geometry/ProgressArray.h index ba644ca12..26dbe82e8 100644 --- a/src/MeasurementComplexItem/geometry/ProgressArray.h +++ b/src/MeasurementComplexItem/geometry/ProgressArray.h @@ -4,7 +4,10 @@ #include #include -typedef std::pair TaggedProgress; -typedef QVector ProgressArray; +#include "comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h" + +typedef ros_bridge::messages::nemo_msgs::labeled_progress::LabeledProgress + LabeledProgress; +typedef QVector ProgressArray; #endif // PROGRESSARRAY_H diff --git a/src/MeasurementComplexItem/nemo_interface/CommandDispatcher.cpp b/src/MeasurementComplexItem/nemo_interface/CommandDispatcher.cpp deleted file mode 100644 index 96a91a114..000000000 --- a/src/MeasurementComplexItem/nemo_interface/CommandDispatcher.cpp +++ /dev/null @@ -1,7 +0,0 @@ -#include "CommandDispatcher.h" - -namespace nemo_interface { - -CommandDispatcher::CommandDispatcher() {} - -} // namespace nemo_interface diff --git a/src/MeasurementComplexItem/nemo_interface/CommandDispatcher.h b/src/MeasurementComplexItem/nemo_interface/CommandDispatcher.h deleted file mode 100644 index 501bdd59f..000000000 --- a/src/MeasurementComplexItem/nemo_interface/CommandDispatcher.h +++ /dev/null @@ -1,31 +0,0 @@ -#ifndef COMMANDDISPATCHER_H -#define COMMANDDISPATCHER_H - -#include -#include - -#include "Task.h" - -namespace nemo_interface { - -class CommandDispatcher : public QThread { -public: - CommandDispatcher(); - - bool interruptionPoint(); // thread safe - - std::future dispatch(const Task &c); // thread safe - std::future dispatchNext(const Task &c); // thread safe - void clear(); // thread safe - void stop(); // thread safe - bool running(); // thread safe - -private: - std::atomic_bool _running; - std::condition_variable _condVar; - QList _queue; - std::mutex _queueMutex; -}; -} // namespace nemo_interface - -#endif // COMMANDDISPATCHER_H diff --git a/src/MeasurementComplexItem/nemo_interface/FutureWatcher.h b/src/MeasurementComplexItem/nemo_interface/FutureWatcher.h new file mode 100644 index 000000000..2e394475e --- /dev/null +++ b/src/MeasurementComplexItem/nemo_interface/FutureWatcher.h @@ -0,0 +1,74 @@ +#ifndef FUTUREWATCHER_H +#define FUTUREWATCHER_H + +#include "FutureWatcherInterface.h" +#include +#include + +namespace nemo_interface { + +//! +//! class similar to QFutureWatcher suitable for a std::future or +//! std::shared_future. \note For single thread use only. +//! +template class Future = std::future> +class FutureWatcher : public FutureWatcherInterface { + enum class STATE { EMPTY, STARTED, FINISHED }; + +public: + typedef Future FutureType; + + explicit FutureWatcher(QObject *parent = nullptr) + : FutureWatcherInterface(parent), _state(STATE::EMPTY) { + _init(); + } + + FutureWatcher(const FutureType &f, QObject *parent = nullptr) + : FutureWatcherInterface(parent), _future(f), _state(STATE::STARTED) { + _init(); + } + + FutureType future() const { return _future; } + + void setFuture(const FutureType &future) { + _future = future; + _state = STATE::STARTED; + _timer.start(1); + emit started(); + } + + T result() { return _future.get(); } + + virtual void waitForFinished() override { + if (_state == STATE::STARTED) { + _timer.stop(); + _future.wait(); + _state = STATE::FINISHED; + } + } + + virtual bool isStarted() override { return _state == STATE::STARTED; } + + virtual bool isFinished() override { return _state == STATE::FINISHED; } + +private: + void _onTimeout() { + if (_state == STATE::STARTED) { + auto status = _future.wait_for(std::chrono::seconds(0)); + if (status == std::future_status::ready) { + _state = STATE::FINISHED; + emit finished(); + } + } + } + + void _init() { + connect(&_timer, &QTimer::timeout, [this] { this->_onTimeout(); }); + } + + FutureType _future; + QTimer _timer; + STATE _state; +}; +} // namespace nemo_interface +#endif // FUTUREWATCHER_H diff --git a/src/MeasurementComplexItem/nemo_interface/FutureWatcherInterface.cpp b/src/MeasurementComplexItem/nemo_interface/FutureWatcherInterface.cpp new file mode 100644 index 000000000..6d55dc407 --- /dev/null +++ b/src/MeasurementComplexItem/nemo_interface/FutureWatcherInterface.cpp @@ -0,0 +1,5 @@ +#include "FutureWatcherInterface.h" +namespace nemo_interface { +FutureWatcherInterface::FutureWatcherInterface(QObject *parent) + : QObject(parent) {} +} // namespace nemo_interface diff --git a/src/MeasurementComplexItem/nemo_interface/FutureWatcherInterface.h b/src/MeasurementComplexItem/nemo_interface/FutureWatcherInterface.h new file mode 100644 index 000000000..a4cf041a1 --- /dev/null +++ b/src/MeasurementComplexItem/nemo_interface/FutureWatcherInterface.h @@ -0,0 +1,23 @@ +#ifndef FUTUREWATCHERINTERFACE_H +#define FUTUREWATCHERINTERFACE_H + +#include + +namespace nemo_interface { + +class FutureWatcherInterface : public QObject { + Q_OBJECT +public: + FutureWatcherInterface(QObject *parent = nullptr); + + virtual void waitForFinished() = 0; + virtual bool isStarted() = 0; + virtual bool isFinished() = 0; + +signals: + void finished(); + void started(); +}; +} // namespace nemo_interface + +#endif // FUTUREWATCHERINTERFACE_H diff --git a/src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp b/src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp index 888d0477f..c63ca002c 100644 --- a/src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp +++ b/src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp @@ -1,5 +1,10 @@ #include "MeasurementTile.h" +#include + +const char *MeasurementTile::settingsGroup = "MeasurementTile"; +const char *MeasurementTile::nameString = "MeasurementTile"; + MeasurementTile::MeasurementTile(QObject *parent) : GeoArea(parent), _progress(0), _id(0) { init(); @@ -31,7 +36,7 @@ void MeasurementTile::push_back(const QGeoCoordinate &c) { this->appendVertex(c); } -void MeasurementTile::init() { this->setObjectName("Tile"); } +void MeasurementTile::init() { this->setObjectName(nameString); } int64_t MeasurementTile::id() const { return _id; } diff --git a/src/MeasurementComplexItem/nemo_interface/MeasurementTile.h b/src/MeasurementComplexItem/nemo_interface/MeasurementTile.h index 490828751..3e08e8206 100644 --- a/src/MeasurementComplexItem/nemo_interface/MeasurementTile.h +++ b/src/MeasurementComplexItem/nemo_interface/MeasurementTile.h @@ -17,7 +17,7 @@ public: virtual QString mapVisualQML() const override; virtual QString editorQML() const override; - virtual MeasurementTile *clone(QObject *parent) const; + virtual MeasurementTile *clone(QObject *parent) const override; void push_back(const QGeoCoordinate &c); @@ -29,6 +29,10 @@ public: QList tile() const; + // Static Variables + static const char *settingsGroup; + static const char *nameString; + signals: void progressChanged(); void idChanged(); diff --git a/src/MeasurementComplexItem/nemo_interface/Task.cpp b/src/MeasurementComplexItem/nemo_interface/Task.cpp index b1033e055..32e976e40 100644 --- a/src/MeasurementComplexItem/nemo_interface/Task.cpp +++ b/src/MeasurementComplexItem/nemo_interface/Task.cpp @@ -2,10 +2,14 @@ namespace nemo_interface { -Task::Task(const Task::Functor &onExec) - : _onExec(onExec), _isExpired([] { return false; }), - _isReady([] { return true; }) {} +Task::Task(const Task::Functor &onExec) : _onExec(onExec) {} -void Task::exec(std::promise p) { this->_onExec(std::move(p)); } +Task::Task(Task::Functor &&onExec) : _onExec(std::move(onExec)) {} + +QVariant Task::exec() { return this->_onExec(); } + +void Task::setOnExec(const Task::Functor &onExec) { _onExec = onExec; } + +void Task::setOnExec(Task::Functor &&onExec) { _onExec = std::move(onExec); } } // namespace nemo_interface diff --git a/src/MeasurementComplexItem/nemo_interface/Task.h b/src/MeasurementComplexItem/nemo_interface/Task.h index 3f3a6361c..f80040844 100644 --- a/src/MeasurementComplexItem/nemo_interface/Task.h +++ b/src/MeasurementComplexItem/nemo_interface/Task.h @@ -10,23 +10,17 @@ namespace nemo_interface { class Task { public: - typedef std::function p)> Functor; - typedef std::function BooleanFunctor; + typedef std::function Functor; Task(const Functor &onExec); + Task(Functor &&onExec); - void exec(std::promise p); - bool isReady(); // default == true - bool isExpired(); // default == false - + QVariant exec(); void setOnExec(const Functor &onExec); - void setIsReady(const BooleanFunctor &isReady); - void setIsExpired(const BooleanFunctor &isExpired); + void setOnExec(Functor &&onExec); private: Functor _onExec; - BooleanFunctor _isExpired; - BooleanFunctor _isReady; }; } // namespace nemo_interface diff --git a/src/MeasurementComplexItem/nemo_interface/TaskDispatcher.cpp b/src/MeasurementComplexItem/nemo_interface/TaskDispatcher.cpp new file mode 100644 index 000000000..94a0f77fd --- /dev/null +++ b/src/MeasurementComplexItem/nemo_interface/TaskDispatcher.cpp @@ -0,0 +1,111 @@ +#include "TaskDispatcher.h" + +#include "QtConcurrent" + +typedef std::unique_lock ULock; + +namespace nemo_interface { + +TaskDispatcher::TaskDispatcher() : _running(false) {} + +TaskDispatcher::~TaskDispatcher() { stop(); } + +std::future TaskDispatcher::dispatch(std::unique_ptr c) { + ULock lk1(this->_mutex); + + this->_taskQueue.push_back(std::move(c)); + std::promise promise; + auto future = promise.get_future(); + this->_promiseQueue.push_back(std::move(promise)); + + if (!this->_running) { + lk1.unlock(); + this->start(); + } + + return future; +} + +std::future TaskDispatcher::dispatchNext(std::unique_ptr c) { + ULock lk1(this->_mutex); + + this->_taskQueue.push_front(std::move(c)); + std::promise promise; + auto future = promise.get_future(); + this->_promiseQueue.push_front(std::move(promise)); + + if (!this->_running) { + lk1.unlock(); + this->start(); + } + + return future; +} + +void TaskDispatcher::clear() { + ULock lk(this->_mutex); + this->_taskQueue.clear(); + this->_promiseQueue.clear(); +} + +void TaskDispatcher::start() { + ULock lk1(this->_mutex); + if (!_running) { + this->_running = true; + lk1.unlock(); + _future = QtConcurrent::run([this]() mutable { return this->run(); }); + } +} + +void TaskDispatcher::stop() { + ULock lk1(this->_mutex); + if (_running) { + this->_running = false; + lk1.unlock(); + this->_future.waitForFinished(); + } +} + +void TaskDispatcher::reset() { + clear(); + stop(); +} + +bool TaskDispatcher::isInterruptionRequested() { + ULock lk1(this->_mutex); + return !_running; +} + +bool TaskDispatcher::isRunning() { + ULock lk1(this->_mutex); + return _running; +} + +std::size_t TaskDispatcher::pendingTasks() { + ULock lk1(this->_mutex); + return this->_taskQueue.size() + (_running ? 1 : 0); +} + +void TaskDispatcher::run() { + while (true) { + ULock lk1(this->_mutex); + + if (this->_taskQueue.size() > 0 && this->_running) { + // pop task and promise + auto pTask = std::move(this->_taskQueue.front()); + auto promise = std::move(this->_promiseQueue.front()); + this->_taskQueue.pop_front(); + this->_promiseQueue.pop_front(); + lk1.unlock(); + + // exec task + promise.set_value(pTask->exec()); + } else { + this->_running = false; + lk1.unlock(); + break; + } + } +} + +} // namespace nemo_interface diff --git a/src/MeasurementComplexItem/nemo_interface/TaskDispatcher.h b/src/MeasurementComplexItem/nemo_interface/TaskDispatcher.h new file mode 100644 index 000000000..d641136bf --- /dev/null +++ b/src/MeasurementComplexItem/nemo_interface/TaskDispatcher.h @@ -0,0 +1,70 @@ +#ifndef COMMANDDISPATCHER_H +#define COMMANDDISPATCHER_H + +#include +#include +#include + +#include "Task.h" + +namespace nemo_interface { + +class TaskDispatcher : QObject { +public: + TaskDispatcher(); + ~TaskDispatcher(); + //! + //! \brief dispatch Dispatches a task. + //! \param c The task to dispatch. + //! \return Returns a future containing the QVariant which the task c + //! returned. + //! + std::future dispatch(std::unique_ptr c); + //! + //! \brief dispatchNext Dispatches the task c as the next task. + //! \param c The task to dispatch. + //! \return Returns a future containing the QVariant which the task c + //! returned. + //! + std::future dispatchNext(std::unique_ptr c); + //! + //! \brief clear Clears the task list. + //! + void clear(); + //! + //! \brief start Starts task dispatching. This function can be called after + //! dispatching has been stopped, e.g. by stop() or reset(). + //! + void start(); + //! + //! \brief stop Stops task dispatching. + //! + //! Stops taks dispatching. The dispatcher will wait for the current task to + //! complete and than terminate the dispatcher thread. After stop() has been + //! called isInterruptionRequested will return true. This can be used to + //! permaturely terminate a task. + //! + void stop(); + //! + //! \brief reset Clears the task list and stops dispatching. + //! + void reset(); + bool isInterruptionRequested(); + bool isRunning(); + + std::size_t pendingTasks(); + +protected: + void run(); + +private: + QFuture _future; + + std::deque> _taskQueue; + std::deque> _promiseQueue; + bool _running; + std::mutex _mutex; +}; +} // namespace nemo_interface + +#endif // COMMANDDISPATCHER_H diff --git a/src/comm/ros_bridge/include/RosBridgeClient.cpp b/src/comm/ros_bridge/include/RosBridgeClient.cpp index 52d638b7b..78ae57a54 100644 --- a/src/comm/ros_bridge/include/RosBridgeClient.cpp +++ b/src/comm/ros_bridge/include/RosBridgeClient.cpp @@ -74,7 +74,8 @@ void RosbridgeWsClient::start(const std::__cxx11::string &client_name, #ifdef ROS_BRIDGE_DEBUG std::thread client_thread([client_name, client]() { #else - std::thread client_thread([client]() { + std::thread client_thread([client_name, client]() { + // std::thread client_thread([client]() { #endif client->start(); diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h b/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h index 6d6a753cf..6c9065060 100644 --- a/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h +++ b/src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h @@ -41,7 +41,6 @@ bool toJson(const HeartbeatType &heartbeat, rapidjson::Value &value, template bool fromJson(const rapidjson::Value &value, HeartbeatType &heartbeat) { if (!value.HasMember("status") || !value["status"].IsInt()) { - assert(false); return false; } diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h b/src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h index c20111918..caff7f369 100644 --- a/src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h +++ b/src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h @@ -24,7 +24,7 @@ const char *idKey = "id"; //! @brief C++ representation of nemo_msgs/labeled_progress message class LabeledProgress { public: - LabeledProgress() {} + LabeledProgress() : _id(0), _progress(0) {} LabeledProgress(double progress, long id) : _id(id), _progress(progress) {} long id() const { return _id; } @@ -52,11 +52,11 @@ bool fromJson(const rapidjson::Value &value, ProgressType &p) { return false; } - if (!value.HasMember(idKey) || !value[idKey].IsInt()) { + if (!value.HasMember(idKey) || !value[idKey].IsInt64()) { return false; } - p.setId(value[idKey].GetInt()); + p.setId(value[idKey].GetInt64()); p.setProgress(value[progressKey].GetDouble()); return true; diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.cpp b/src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.cpp new file mode 100644 index 000000000..bab6ddfbb --- /dev/null +++ b/src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.cpp @@ -0,0 +1,5 @@ +#include "progress_array.h" + +std::string ros_bridge::messages::nemo_msgs::progress_array::messageType() { + return "/nemo_msgs/ProgressArray"; +} diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.h b/src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.h new file mode 100644 index 000000000..e3d3299ab --- /dev/null +++ b/src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.h @@ -0,0 +1,88 @@ +#ifndef PROGRESS_ARRAY_H +#define PROGRESS_ARRAY_H + +#pragma once + +#include "labeled_progress.h" +#include "ros_bridge/rapidjson/include/rapidjson/document.h" + +#include + +namespace ros_bridge { +//! @brief Namespace containing classes and methodes ros message generation. +namespace messages { +//! @brief Namespace containing classes and methodes for geometry_msgs +//! generation. +namespace nemo_msgs { +//! @brief Namespace containing methodes for nemo_msgs/progress_array message +//! generation. +namespace progress_array { + +std::string messageType(); + +namespace { +const char *progressArrayKey = "progress_array"; +} // namespace + +//! @brief C++ representation of nemo_msgs/progress_array message +template