version checking added to nemo inteface, testing needed..

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 += \

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<IDArray> 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<QVector<std::shared_ptr<Tile>>> pTileArray,
                    std::promise<bool> promise);
+  void _setTiles(std::shared_ptr<QVector<std::shared_ptr<Tile>>> pTileArray,
+                 std::promise<bool> promise);
+  void _setVersion(QString version, std::promise<bool> promise);
   void _removeTilesRemote(std::shared_ptr<IDArray> idArray,
                           std::promise<bool> promise);
   void _clearTilesRemote(std::promise<bool> promise);
@@ -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> _state;
+  std::atomic_bool _versionOK;
+  std::atomic_bool _progressTopicOK;
+  std::atomic_bool _heartbeatTopicOK;
   std::atomic<CALL_NAME> _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<NemoInterface::STATUS, QString>;
 static StatusMap statusMap{
     std::make_pair<NemoInterface::STATUS, QString>(
         NemoInterface::STATUS::NOT_CONNECTED, "Not Connected"),
+    std::make_pair<NemoInterface::STATUS, QString>(NemoInterface::STATUS::ERROR,
+                                                   "ERROR"),
     std::make_pair<NemoInterface::STATUS, QString>(NemoInterface::STATUS::SYNC,
                                                    "Synchronizing"),
     std::make_pair<NemoInterface::STATUS, QString>(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<IDArray>();
-    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<QVariant> 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<bool> 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<nemo_interface::Task>([this] {
+    // wait for service
+    std::future<bool> 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<nemo_interface::Task>([this] {
+    // wait for service
+    std::future<bool> 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<ProgressArray>();
-          *p = std::move(progressArray.progress_array());
-          std::promise<bool> promise;
-          auto future = promise.get_future();
-          bool value = QMetaObject::invokeMethod(
-              this->_parent, [this, p, promise = std::move(promise)]() mutable {
-                this->_updateProgress(p, std::move(promise));
-              });
-          Q_ASSERT(value == true);
-          future.wait();
-        } 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<bool> promise;
-          auto future = promise.get_future();
-          bool value = QMetaObject::invokeMethod(
-              this->_parent,
-              [this, heartbeat, promise = std::move(promise)]() mutable {
-                this->_onHeartbeatReceived(heartbeat, std::move(promise));
-              });
-          Q_ASSERT(value == true);
-          future.wait();
-        } else {
-          qCWarning(NemoInterfaceLog) << "/nemo/heartbeat not able to "
-                                         "create Heartbeat form json: "
-                                      << o;
+        auto p = std::make_shared<ProgressArray>();
+        *p = std::move(progressArray.progress_array());
+        std::promise<bool> promise;
+        auto future = promise.get_future();
+        bool value = QMetaObject::invokeMethod(
+            this->_parent, [this, p, promise = std::move(promise)]() mutable {
+              this->_updateProgress(p, std::move(promise));
+            });
+        Q_ASSERT(value == true);
+        future.wait();
+      } 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<nemo_interface::Task>([this] {
+    // wait for service
+    std::future<bool> 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<bool> promise;
+            auto future = promise.get_future();
+            bool value = QMetaObject::invokeMethod(
+                this->_parent,
+                [this, heartbeat, promise = std::move(promise)]() mutable {
+                  this->_onHeartbeatReceived(heartbeat, std::move(promise));
+                });
+            Q_ASSERT(value == true);
+            future.wait();
+          } else {
+            qCWarning(NemoInterfaceLog) << "/nemo/heartbeat not able to "
+                                           "create Heartbeat form json: "
+                                        << o;
+          }
+        });
+
+    this->_heartbeatTopicOK = true;
+    bool value = QMetaObject::invokeMethod(this->_parent /* context */,
+                                           [this]() { this->_doAction(); });
+    Q_ASSERT(value == true);
+
+    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<QVector<std::shared_ptr<const Tile>>>();
-    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<nemo_interface::Task>(
+        [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<bool>());
     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);