From 7a0b738bdeedfa22f866b5c8155868ed96a7d16e Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Tue, 26 Jan 2021 15:46:15 +0100 Subject: [PATCH] version checking added to nemo inteface, testing needed.. --- qgroundcontrol.pro | 2 + src/MeasurementComplexItem/NemoInterface.cpp | 603 +++++++++++++++--- src/MeasurementComplexItem/NemoInterface.h | 1 + .../nemo_interface/MeasurementTile.cpp | 11 +- src/MeasurementComplexItem/qml/NemoEditor.qml | 11 +- .../rosbridge/rosbridge.cpp | 12 +- .../rosbridge/rosbridge.h | 2 - .../rosbridge/rosbridgeimpl.cpp | 6 +- .../rosbridge/rosbridgeimpl.h | 1 + .../include/messages/nemo_msgs/tile.h | 2 +- .../include/messages/nemo_msgs/tile_array.cpp | 1 + .../include/messages/nemo_msgs/tile_array.h | 55 ++ 12 files changed, 596 insertions(+), 111 deletions(-) create mode 100644 src/comm/ros_bridge/include/messages/nemo_msgs/tile_array.cpp create mode 100644 src/comm/ros_bridge/include/messages/nemo_msgs/tile_array.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index dcd246db8..656a58a52 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -517,6 +517,7 @@ HEADERS += \ 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/nemo_msgs/tile_array.h \ src/comm/utilities.h contains (DEFINES, QGC_ENABLE_PAIRING) { @@ -559,6 +560,7 @@ SOURCES += \ src/comm/ros_bridge/include/messages/nemo_msgs/progress_array.cpp \ src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp \ src/Settings/WimaSettings.cc \ + src/comm/ros_bridge/include/messages/nemo_msgs/tile_array.cpp contains (DEFINES, QGC_ENABLE_PAIRING) { SOURCES += \ diff --git a/src/MeasurementComplexItem/NemoInterface.cpp b/src/MeasurementComplexItem/NemoInterface.cpp index 69d176252..beed9c55f 100644 --- a/src/MeasurementComplexItem/NemoInterface.cpp +++ b/src/MeasurementComplexItem/NemoInterface.cpp @@ -26,12 +26,16 @@ #include "ros_bridge/include/messages/nemo_msgs/heartbeat.h" #include "ros_bridge/include/messages/nemo_msgs/progress_array.h" #include "ros_bridge/include/messages/nemo_msgs/tile.h" +#include "ros_bridge/include/messages/nemo_msgs/tile_array.h" #include "rosbridge/rosbridge.h" QGC_LOGGING_CATEGORY(NemoInterfaceLog, "NemoInterfaceLog") -#define SYNC_INTERVAL 1000 // ms -#define NO_HEARTBEAT_TIMEOUT 5000 // ms +#define NO_HEARTBEAT_TIMEOUT 5000 // ms +#define RESTART_INTERVAl 600000 // ms == 10 min +#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"; @@ -55,7 +59,7 @@ class NemoInterface::Impl { STOPPED, START_BRIDGE, WEBSOCKET_DETECTED, - TRY_TOPIC_SERVICE_SETUP, + TRY_SETUP, USER_SYNC, SYS_SYNC, READY, @@ -96,8 +100,13 @@ public: private: void _doTopicServiceSetup(); + void _checkVersion(); + void _subscribeProgressTopic(); + void _subscribeHearbeatTopic(); void _doAction(); void _trySynchronize(); + void _synchronizeIfNeccessary(); + void _tryRestart(); bool _isSynchronized() const; bool _userSync() const; // thread safe bool _sysSync() const; // thread safe @@ -115,12 +124,16 @@ private: // 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_PROGRESS + GET_ALL_TILES, + GET_ALL_PROGRESS, + GET_VERSION }; QString _toString(CALL_NAME name); @@ -130,6 +143,9 @@ private: void _addTilesRemote2(std::shared_ptr>> pTileArray, std::promise promise); + void _setTiles(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); @@ -141,6 +157,7 @@ private: 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); @@ -149,7 +166,13 @@ private: static QString _toString(STATE s); static QString _toString(NemoInterface::STATUS s); + static QString _localVersion; + QString _remoteVersion; + std::atomic _state; + std::atomic_bool _versionOK; + std::atomic_bool _progressTopicOK; + std::atomic_bool _heartbeatTopicOK; std::atomic _lastCall; ROSBridgePtr _pRosbridge; TileMap _remoteTiles; @@ -159,14 +182,20 @@ private: QString _infoString; QString _warningString; QTimer _timeoutTimer; + QTimer _syncTimer; + QTimer _restartTimer; QNemoHeartbeat _lastHeartbeat; FutureWatcher _futureWatcher; }; +QString NemoInterface::Impl::_localVersion("V_1.0"); + using StatusMap = std::map; static StatusMap statusMap{ std::make_pair( NemoInterface::STATUS::NOT_CONNECTED, "Not Connected"), + std::make_pair(NemoInterface::STATUS::ERROR, + "ERROR"), std::make_pair(NemoInterface::STATUS::SYNC, "Synchronizing"), std::make_pair(NemoInterface::STATUS::READY, @@ -177,7 +206,8 @@ static StatusMap statusMap{ NemoInterface::STATUS::WEBSOCKET_DETECTED, "Websocket Detected")}; NemoInterface::Impl::Impl(NemoInterface *p) - : _state(STATE::STOPPED), _parent(p) { + : _state(STATE::STOPPED), _versionOK(false), _progressTopicOK(false), + _heartbeatTopicOK(false), _parent(p) { // ROS Bridge. WimaSettings *wimaSettings = @@ -202,12 +232,17 @@ NemoInterface::Impl::Impl(NemoInterface *p) connect(&this->_timeoutTimer, &QTimer::timeout, std::bind(&Impl::_onHeartbeatTimeout, this)); - // Connection timer (temporary workaround) - connect(this->_pRosbridge.get(), &Rosbridge::stateChanged, + connect(this->_pRosbridge.get(), &Rosbridge::stateChanged, this->_parent, [this] { this->_onRosbridgeStateChanged(); }); - connect(&this->_futureWatcher, &FutureWatcher::finished, + 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(); }); } NemoInterface::Impl::~Impl() { this->_pRosbridge->stop(); } @@ -253,7 +288,7 @@ NemoInterface::Impl::addTiles(const TilePtrArray &tileArray) { } } if (pTileArray->size() > 0) { - this->_parent->tilesChanged(); + emit this->_parent->tilesChanged(); } // ready for send? @@ -303,7 +338,7 @@ NemoInterface::Impl::removeTiles(const IDArray &idArray) { // copy known ids auto pIdArray = std::make_shared(); - for (const auto id : idArray) { + for (const auto &id : idArray) { const auto it = this->_localTiles.find(id); Q_ASSERT(it != _localTiles.end()); if (Q_LIKELY(it != _localTiles.end())) { @@ -314,7 +349,7 @@ NemoInterface::Impl::removeTiles(const IDArray &idArray) { } } if (pIdArray->size() > 0) { - this->_parent->tilesChanged(); + emit this->_parent->tilesChanged(); } // ready for send? @@ -348,7 +383,7 @@ std::shared_future NemoInterface::Impl::clearTiles() { // clear local tiles (_localTiles) if (!_localTiles.empty()) { this->_localTiles.clear(); - this->_parent->tilesChanged(); + emit this->_parent->tilesChanged(); } if (this->ready() || this->_userSync()) { @@ -498,11 +533,23 @@ bool NemoInterface::Impl::_sysSync() const { return _sysSync(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); - QTimer::singleShot(5000, [this] { this->_trySynchronize(); }); + + 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; } } } @@ -521,8 +568,7 @@ void NemoInterface::Impl::_onRosbridgeStateChanged() { this->_doAction(); } } else if (state == Rosbridge::STATE::TIMEOUT) { - if (this->_state == STATE::TRY_TOPIC_SERVICE_SETUP || - this->_state == STATE::READY || + 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); @@ -570,7 +616,7 @@ void NemoInterface::Impl::_onHeartbeatReceived(const QNemoHeartbeat &hb, std::promise promise) { _lastHeartbeat = hb; this->_timeoutTimer.start(NO_HEARTBEAT_TIMEOUT); - if (this->_state == STATE::TRY_TOPIC_SERVICE_SETUP) { + if (this->_state == STATE::TRY_SETUP) { this->_setState(STATE::READY); this->_doAction(); } else if (this->_state == STATE::HEARTBEAT_TIMEOUT) { @@ -594,71 +640,197 @@ void NemoInterface::Impl::_setWarningString(const QString &warning) { } } -void NemoInterface::Impl::_doTopicServiceSetup() { - using namespace ros_bridge::messages; - - // Subscribe nemo progress. - this->_pRosbridge->subscribeTopic( - progressTopic, [this](const QJsonObject &o) { - nemo_msgs::progress_array::ProgressArray progressArray; - if (nemo_msgs::progress_array::fromJson(o, 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(); - } +void NemoInterface::Impl::_doTopicServiceSetup() {} + +void NemoInterface::Impl::_checkVersion() { + + auto pTask = std::make_unique([this] { + // wait for service + std::future fut; + long tries = 0; + long maxTries = 50; + 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; + } + } + ++tries; + if (tries > maxTries) { + qCWarning(NemoInterfaceLog) + << "_checkVersion: service /nemo/get_version not available."; + bool value = + QMetaObject::invokeMethod(this->_parent /* context */, [this]() { + this->_setWarningString("Version checking failed."); + }); + Q_ASSERT(value == true); + return QVariant(false); + } + } while (!fut.get()); + + // call service + return this->_callGetVersion(); + }); + + this->_dispatcher.dispatch(std::move(pTask)); +} + +void NemoInterface::Impl::_subscribeProgressTopic() { + auto pTask = std::make_unique([this] { + // wait for service + std::future fut; + long tries = 0; + long maxTries = 50; + 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; + } + } + ++tries; + if (tries > maxTries) { + qCWarning(NemoInterfaceLog) + << "_subscribeProgressTopic: topic /nemo/progress not available."; + bool value = + QMetaObject::invokeMethod(this->_parent /* context */, [this]() { + this->_setWarningString("Progress topic not available."); + }); + Q_ASSERT(value == true); + return QVariant(false); + } + } while (!fut.get()); + + // subscribe + this->_pRosbridge->subscribeTopic(progressTopic, [this]( + const QJsonObject &o) { + ros_bridge::messages::nemo_msgs::progress_array::ProgressArray + progressArray; + if (ros_bridge::messages::nemo_msgs::progress_array::fromJson( + o, 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; } - 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(); - } else { - qCWarning(NemoInterfaceLog) << "/nemo/progress not able to " - "create ProgressArray form json: " - << o; + if (rangeError) { + qCWarning(NemoInterfaceLog) << "/nemo/progress progress out " + "of range, value was set to: " + << lp.progress(); + } } - }); - // Subscribe heartbeat msg. - 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; + 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(); + } else { + qCWarning(NemoInterfaceLog) << "/nemo/progress 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; + long tries = 0; + long maxTries = 50; + 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; + } + } + ++tries; + if (tries > maxTries) { + qCWarning(NemoInterfaceLog) + << "_subscribeHeartbeatTopic: topic /nemo/hearbeat not available."; + bool value = + QMetaObject::invokeMethod(this->_parent /* context */, [this]() { + this->_setWarningString("Heartbeat topic not available."); + }); + Q_ASSERT(value == true); + return QVariant(false); + } + } 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); + + return QVariant(true); + }); + + this->_dispatcher.dispatch(std::move(pTask)); } void NemoInterface::Impl::_trySynchronize() { @@ -667,7 +839,9 @@ void NemoInterface::Impl::_trySynchronize() { !_isSynchronized()) { if (!_dispatcher.idle()) { - QTimer::singleShot(5000, [this] { this->_trySynchronize(); }); + QTimer::singleShot(SYNC_RETRY_INTERVAL, this->_parent, + [this] { this->_trySynchronize(); }); + qCWarning(NemoInterfaceLog) << "synchronization defered"; return; } @@ -687,7 +861,7 @@ void NemoInterface::Impl::_trySynchronize() { // create tile array. auto pTileArray = std::make_shared>>(); - for (auto pair : _localTiles) { + for (const auto &pair : _localTiles) { pTileArray->push_back(pair.second); } @@ -722,6 +896,35 @@ void NemoInterface::Impl::_trySynchronize() { } } +void NemoInterface::Impl::_synchronizeIfNeccessary() { + if (_dispatcher.idle()) { + // create getAllTiles cmd. + auto pTask = std::make_unique( + [this] { return this->_callGetAllTiles(); }); + + // dispatch command. + auto ret = _dispatcher.dispatch(std::move(pTask)); + auto fut = ret.share(); + _futureWatcher.setFuture(fut); + _syncTimer.start(SYNC_INTERVAL); + } else { + _syncTimer.start(SYNC_RETRY_INTERVAL); + } +} + +void NemoInterface::Impl::_tryRestart() { + if (this->running()) { + if (_dispatcher.idle()) { + qDebug() << "_tryRestart: restarting"; + this->stop(); + this->start(); + _restartTimer.start(RESTART_INTERVAl); + } else { + _restartTimer.start(RESTART_RETRY_INTERVAl); + } + } +} + bool NemoInterface::Impl::_isSynchronized() const { return _localTiles.size() > 0 && _remoteTiles.size() > 0 && std::equal( @@ -733,33 +936,50 @@ void NemoInterface::Impl::_doAction() { 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()); if (this->_pRosbridge->state() != Rosbridge::STATE::STOPPED) { this->_pRosbridge->stop(); } + _versionOK = false; + _progressTopicOK = false; + _heartbeatTopicOK = false; break; case STATE::START_BRIDGE: this->_pRosbridge->start(); + this->_restartTimer.start(RESTART_INTERVAl); break; case STATE::WEBSOCKET_DETECTED: resetDone = false; - this->_setState(STATE::TRY_TOPIC_SERVICE_SETUP); + this->_setState(STATE::TRY_SETUP); this->_doAction(); break; - case STATE::TRY_TOPIC_SERVICE_SETUP: - this->_doTopicServiceSetup(); - this->_timeoutTimer.start(NO_HEARTBEAT_TIMEOUT); + 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: - _trySynchronize(); + this->_trySynchronize(); + this->_syncTimer.start(SYNC_INTERVAL); break; case STATE::USER_SYNC: case STATE::SYS_SYNC: break; case STATE::HEARTBEAT_TIMEOUT: this->_clearTilesRemote(std::promise()); + this->_syncTimer.stop(); break; case STATE::WEBSOCKET_TIMEOUT: if (!resetDone) { @@ -768,7 +988,11 @@ void NemoInterface::Impl::_doAction() { this->_pRosbridge->start(); } this->_timeoutTimer.stop(); + this->_syncTimer.stop(); this->_clearTilesRemote(std::promise()); + _versionOK = false; + _progressTopicOK = false; + _heartbeatTopicOK = false; break; }; } @@ -782,7 +1006,7 @@ QVariant NemoInterface::Impl::_callAddTiles( // create json object QJsonArray jsonTileArray; - for (const auto &tile : *pTileArray) { + for (auto &&tile : *pTileArray) { using namespace ros_bridge::messages; QJsonObject jsonTile; if (!nemo_msgs::tile::toJson(*tile, jsonTile)) { @@ -859,7 +1083,7 @@ NemoInterface::Impl::_callRemoveTiles(std::shared_ptr pIdArray) { // create json object QJsonArray jsonIdArray; - for (const auto id : *pIdArray) { + for (auto &&id : *pIdArray) { using namespace ros_bridge::messages; jsonIdArray.append(id); } // for @@ -929,7 +1153,7 @@ QVariant NemoInterface::Impl::_callClearTiles() { // create response handler. auto promise_response = std::make_shared>(); auto future_response = promise_response->get_future(); - auto responseHandler = [promise_response](const QJsonObject &o) mutable { + auto responseHandler = [promise_response](const QJsonObject &) mutable { // check if transaction was successfull promise_response->set_value(true); }; @@ -981,7 +1205,7 @@ NemoInterface::Impl::_callGetProgress(std::shared_ptr pIdArray) { // create json object QJsonArray jsonIdArray; - for (const auto id : *pIdArray) { + for (auto &&id : *pIdArray) { using namespace ros_bridge::messages; jsonIdArray.append(id); } // for @@ -1117,9 +1341,153 @@ QVariant NemoInterface::Impl::_callGetAllProgress() { return QVariant(future.get()); } +QVariant NemoInterface::Impl::_callGetAllTiles() { + // qDebug() << "_callGetAllProgress called"; + + this->_lastCall = CALL_NAME::GET_ALL_TILES; + + // 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](const QJsonObject &o) mutable { + const char *const tileArrayKey = "tile_array"; + // check if transaction was successfull + if (o.contains(tileArrayKey) && o[tileArrayKey].isArray()) { + auto pArray = std::make_shared>>(); + const auto jsonArray = o[tileArrayKey].toArray(); + for (int i = 0; i < jsonArray.size(); ++i) { + if (jsonArray[i].isObject()) { + QJsonObject o = jsonArray[i].toObject(); + auto tile = std::make_shared(); + if (ros_bridge::messages::nemo_msgs::tile::fromJson(o, *tile)) { + pArray->push_back(tile); + } else { + qCWarning(NemoInterfaceLog) + << "/nemo/get_all_tiles error while creating tile."; + promise_response->set_value(nullptr); + } + } else { + qCWarning(NemoInterfaceLog) + << "/nemo/get_all_tiles json array does not contain objects."; + promise_response->set_value(nullptr); + } + } + // success! + promise_response->set_value(pArray); + } else { + qCWarning(NemoInterfaceLog) + << "/nemo/get_all_tiles no tile_array key or wrong type."; + promise_response->set_value(nullptr); + } + }; + + // call service. + this->_pRosbridge->callService("/nemo/get_all_tiles", responseHandler, + QJsonObject()); + + // 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) + << "all_remove_tiles(): Websocket not responding to request."; + return QVariant(false); + } + + // 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->_setTiles(pArray, std::move(promise)); + }); + Q_ASSERT(value == true); + + // return success + return QVariant(future.get()); +} + +QVariant NemoInterface::Impl::_callGetVersion() { + this->_lastCall = CALL_NAME::GET_VERSION; + + // create response handler. + typedef QString ResponseType; + auto promise_response = std::make_shared>(); + auto future_response = promise_response->get_future(); + + auto responseHandler = [promise_response](const QJsonObject &o) mutable { + const char *const versionKey = "version"; + // check if transaction was successfull + if (o.contains(versionKey) && o[versionKey].isString()) { + const auto version = o[versionKey].toString(); + promise_response->set_value(version); + } else { + qCWarning(NemoInterfaceLog) + << "/nemo/get_version no version key or wrong type."; + promise_response->set_value("error!"); + } + }; + + // call service. + this->_pRosbridge->callService("/nemo/get_version", responseHandler, + QJsonObject()); + + // 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) + << "all_remove_tiles(): Websocket not responding to request."; + return QVariant(false); + } + + // transaction error? + auto version = future_response.get(); + if (version == "error!") { + return QVariant(false); + } + + // 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: @@ -1130,6 +1498,10 @@ QString NemoInterface::Impl::_toString(NemoInterface::Impl::CALL_NAME name) { 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"); } @@ -1141,7 +1513,7 @@ void NemoInterface::Impl::_addTilesRemote( // qDebug() << "_addTilesRemote called"; auto pArrayDup = std::make_shared>>(); - for (auto pTile : *pTileArray) { + for (auto &&pTile : *pTileArray) { pArrayDup->push_back(std::make_shared(*pTile)); } _addTilesRemote2(pArrayDup, std::move(promise)); @@ -1156,7 +1528,7 @@ void NemoInterface::Impl::_addTilesRemote2( bool anyChange = false; bool error = false; - for (auto pTile : *pTileArray) { + for (auto &&pTile : *pTileArray) { auto id = pTile->id(); auto it = _remoteTiles.find(id); if (Q_LIKELY(it == _remoteTiles.end())) { @@ -1183,12 +1555,55 @@ void NemoInterface::Impl::_addTilesRemote2( promise.set_value(!error); } +void NemoInterface::Impl::_setTiles( + std::shared_ptr>> pTileArray, + std::promise promise) { + + bool error = false; + + _remoteTiles.clear(); + for (auto &&pTile : *pTileArray) { + auto id = pTile->id(); + auto it = _remoteTiles.find(id); + if (Q_LIKELY(it == _remoteTiles.end())) { + auto ret = _remoteTiles.insert(std::make_pair(id, pTile)); + Q_ASSERT(ret.second == true); + Q_UNUSED(ret); + } else { + qCWarning(NemoInterfaceLog) + << "_addTilesRemote: tile with id " << id << " already added."; + error = true; + } + } + + if (error || !this->_isSynchronized()) { + qDebug() << "_setTiles: trying to synchronize"; + _trySynchronize(); + } + + promise.set_value(true); +} + +void NemoInterface::Impl::_setVersion(QString version, + std::promise promise) { + _remoteVersion = version; + if (_remoteVersion != _localVersion) { + _setWarningString("Local protocol version (" + _localVersion + + ") does not match remote version (" + _remoteVersion + + ")."); + } else { + _versionOK = true; + _doAction(); + } + promise.set_value(true); +} + void NemoInterface::Impl::_removeTilesRemote(std::shared_ptr idArray, std::promise promise) { // qDebug() << "_removeTilesRemote called"; bool anyChange = false; - for (const auto id : *idArray) { + for (auto &&id : *idArray) { auto it = _remoteTiles.find(id); if (Q_LIKELY(it != _remoteTiles.end())) { _remoteTiles.erase(it); @@ -1273,7 +1688,7 @@ NemoInterface::Impl::_status(NemoInterface::Impl::STATE state) { case STATE::WEBSOCKET_DETECTED: status = NemoInterface::STATUS::WEBSOCKET_DETECTED; break; - case STATE::TRY_TOPIC_SERVICE_SETUP: + case STATE::TRY_SETUP: status = NemoInterface::STATUS::WEBSOCKET_DETECTED; break; case STATE::READY: @@ -1300,7 +1715,7 @@ QString NemoInterface::Impl::_toString(NemoInterface::Impl::STATE s) { return QString("START_BRIDGE"); case STATE::WEBSOCKET_DETECTED: return QString("WEBSOCKET_DETECTED"); - case STATE::TRY_TOPIC_SERVICE_SETUP: + case STATE::TRY_SETUP: return QString("TRY_TOPIC_SERVICE_SETUP"); case STATE::READY: return QString("READY"); @@ -1320,6 +1735,8 @@ QString NemoInterface::Impl::_toString(NemoInterface::STATUS s) { switch (s) { case NemoInterface::STATUS::NOT_CONNECTED: return QString("NOT_CONNECTED"); + case NemoInterface::STATUS::ERROR: + return QString("ERROR"); case NemoInterface::STATUS::READY: return QString("READY"); case NemoInterface::STATUS::TIMEOUT: diff --git a/src/MeasurementComplexItem/NemoInterface.h b/src/MeasurementComplexItem/NemoInterface.h index 4cb4b66e7..93180efca 100644 --- a/src/MeasurementComplexItem/NemoInterface.h +++ b/src/MeasurementComplexItem/NemoInterface.h @@ -30,6 +30,7 @@ public: enum class STATUS { NOT_CONNECTED, + ERROR, SYNC, READY, WEBSOCKET_DETECTED, diff --git a/src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp b/src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp index 1d8735330..afe22a591 100644 --- a/src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp +++ b/src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp @@ -52,13 +52,22 @@ QList MeasurementTile::tile() const { return coordinateList(); } QString MeasurementTile::randomId(int length) { static const QString values( "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789"); + static std::uint64_t counter = 0; + static auto firstCall = std::chrono::high_resolution_clock::now(); + + auto delta = std::chrono::duration_cast( + std::chrono::high_resolution_clock::now() - firstCall) + .count(); + + std::srand((unsigned int)delta ^ counter); QString str; - std::srand(std::time(nullptr)); for (int i = 0; i < length; ++i) { int index = std::rand() % values.length(); QChar c = values.at(index); str.append(c); } + + ++counter; return str; } diff --git a/src/MeasurementComplexItem/qml/NemoEditor.qml b/src/MeasurementComplexItem/qml/NemoEditor.qml index 26b36a9fe..0629c98b2 100644 --- a/src/MeasurementComplexItem/qml/NemoEditor.qml +++ b/src/MeasurementComplexItem/qml/NemoEditor.qml @@ -22,7 +22,8 @@ Rectangle { property int availableWidth: 300 property bool error: errorString.lenght >= 0 readonly property bool running: _nemoInterface.running - property string errorString: "" + property string warningString: _nemoInterface.warningString + property string infoString: _nemoInterface.infoString signal abort @@ -50,7 +51,13 @@ Rectangle { color: "orange" Layout.columnSpan: parent.columns Layout.fillWidth: true - visible: !_root.areasCorrect + } + QGCLabel { + text: _root.infoString + wrapMode: Text.WordWrap + horizontalAlignment: Text.AlignLeft + Layout.columnSpan: parent.columns + Layout.fillWidth: true } QGCButton { diff --git a/src/MeasurementComplexItem/rosbridge/rosbridge.cpp b/src/MeasurementComplexItem/rosbridge/rosbridge.cpp index 9c2efe949..6290445af 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::_onImplStateChanged); + &Rosbridge::stateChanged); connect(this, &Rosbridge::_start, _impl, &RosbridgeImpl::start); connect(this, &Rosbridge::_stop, _impl, &RosbridgeImpl::stop); @@ -244,16 +244,6 @@ void Rosbridge::waitForService(const QString &service) { } } -void Rosbridge::_onImplStateChanged() { - static STATE oldState = STATE::STOPPED; - auto newState = translate(_impl->state()); - - if (oldState != newState) { - emit stateChanged(); - } - oldState = newState; -} - Rosbridge::STATE translate(RosbridgeImpl::STATE s) { switch (s) { case RosbridgeImpl::STATE::STOPPED: diff --git a/src/MeasurementComplexItem/rosbridge/rosbridge.h b/src/MeasurementComplexItem/rosbridge/rosbridge.h index 2be1b8c4f..f3d70354e 100644 --- a/src/MeasurementComplexItem/rosbridge/rosbridge.h +++ b/src/MeasurementComplexItem/rosbridge/rosbridge.h @@ -101,8 +101,6 @@ signals: void _unadvertiseAllServices(); private: - void _onImplStateChanged(); - std::atomic _state; RosbridgeImpl *_impl; QThread *_t; diff --git a/src/MeasurementComplexItem/rosbridge/rosbridgeimpl.cpp b/src/MeasurementComplexItem/rosbridge/rosbridgeimpl.cpp index 25b591d0c..5b1ca5cb3 100644 --- a/src/MeasurementComplexItem/rosbridge/rosbridgeimpl.cpp +++ b/src/MeasurementComplexItem/rosbridge/rosbridgeimpl.cpp @@ -337,6 +337,11 @@ void RosbridgeImpl::_onDisconnected() { } } +void RosbridgeImpl::_onError(QAbstractSocket::SocketError e) { + qDebug() << "_onError: socket error: " << e << ", " + << _webSocket.errorString(); +} + void RosbridgeImpl::_setState(RosbridgeImpl::STATE newState) { if (_state != newState) { _state = newState; @@ -372,7 +377,6 @@ void RosbridgeImpl::_doAction() { } void RosbridgeImpl::_onTextMessageReceived(const QString &message) { - qDebug() << "_onTextMessageReceived: " << message; QJsonParseError e; auto d = QJsonDocument::fromJson(message.toUtf8(), &e); if (!d.isNull()) { diff --git a/src/MeasurementComplexItem/rosbridge/rosbridgeimpl.h b/src/MeasurementComplexItem/rosbridge/rosbridgeimpl.h index e5e7ff85f..605550c9b 100644 --- a/src/MeasurementComplexItem/rosbridge/rosbridgeimpl.h +++ b/src/MeasurementComplexItem/rosbridge/rosbridgeimpl.h @@ -58,6 +58,7 @@ signals: private slots: void _onConnected(); void _onDisconnected(); + void _onError(QAbstractSocket::SocketError e); void _doAction(); void _onTextMessageReceived(const QString &message); diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/tile.h b/src/comm/ros_bridge/include/messages/nemo_msgs/tile.h index 7c7c59383..bc34bbae2 100644 --- a/src/comm/ros_bridge/include/messages/nemo_msgs/tile.h +++ b/src/comm/ros_bridge/include/messages/nemo_msgs/tile.h @@ -100,7 +100,7 @@ template bool fromJson(const QJsonObject &value, TileType &p) { PointType; for (long i = 0; i < jsonArray.size(); ++i) { PointType pt; - if (!geo_point::fromJson(jsonArray[i], pt)) { + if (!geo_point::fromJson(jsonArray[i].toObject(), pt)) { return false; } p.tile().push_back(std::move(pt)); diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/tile_array.cpp b/src/comm/ros_bridge/include/messages/nemo_msgs/tile_array.cpp new file mode 100644 index 000000000..933886a80 --- /dev/null +++ b/src/comm/ros_bridge/include/messages/nemo_msgs/tile_array.cpp @@ -0,0 +1 @@ +#include "tile_array.h" diff --git a/src/comm/ros_bridge/include/messages/nemo_msgs/tile_array.h b/src/comm/ros_bridge/include/messages/nemo_msgs/tile_array.h new file mode 100644 index 000000000..49e33a918 --- /dev/null +++ b/src/comm/ros_bridge/include/messages/nemo_msgs/tile_array.h @@ -0,0 +1,55 @@ +#pragma once + +#include "ros_bridge/include/messages/nemo_msgs/tile.h" + +#include +#include + +namespace ros_bridge { +//! @brief Namespace containing classes and methodes ros message generation. +namespace messages { +//! @brief Namespace containing classes and methodes for nemo_msgs +//! generation. +namespace nemo_msgs { +//! @brief Namespace containing methodes for nemo_msgs/tile_array message +//! generation. +namespace tile_array { + +template +bool toJson(const TileArray &array, QJsonArray &jsonArray) { + + for (unsigned long i = 0; i < std::uint64_t(array.size()); ++i) { + QJsonObject jsonTile; + if (!tile::toJson(array[i], jsonTile)) { + return false; + } + jsonArray.push_back(std::move(jsonTile)); + } + + return true; +} + +template +bool fromJson(const QJsonArray &jsonArray, TileArray &array) { + using namespace geographic_msgs; + + array.clear(); + array.reserve(jsonArray.size()); + typedef decltype(array[0]) TileCVR; + typedef typename std::remove_cv_t> + Tile; + for (long i = 0; i < jsonArray.size(); ++i) { + Tile t; + if (!tile::fromJson(jsonArray[i], t)) { + return false; + } + array.push_back(std::move(t)); + } + + return true; +} + +} // namespace tile_array +} // namespace nemo_msgs +} // namespace messages +} // namespace ros_bridge -- 2.22.0