tile id int64_t > QString, for robustness

qgroundcontrol.pro

@@ -517,8 +517,6 @@ 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/std_msgs/header.h \
-    src/comm/ros_bridge/include/messages/std_msgs/time.h \
     src/comm/utilities.h
 
 contains (DEFINES, QGC_ENABLE_PAIRING) {
@@ -560,8 +558,6 @@ SOURCES += \
     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 \
     src/Settings/WimaSettings.cc \
 
 contains (DEFINES, QGC_ENABLE_PAIRING) {

src/MeasurementComplexItem/IDArray.h

@@ -3,6 +3,6 @@
 
 #include <QVector>
 
-typedef QVector<std::int64_t> IDArray;
+typedef QVector<QString> IDArray;
 
 #endif // IDARRAY_H

src/MeasurementComplexItem/MeasurementComplexItem.h

@@ -10,7 +10,7 @@
 #include "SettingsFact.h"
 
 #include "AreaData.h"
-#include "ProgressArray.h"
+#include "geometry/ProgressArray.h"
 
 class RoutingThread;
 class RoutingResult;

src/MeasurementComplexItem/NemoInterface.cpp

@@ -9,6 +9,8 @@
 
 #include <shared_mutex>
 
+#include <QJsonArray>
+#include <QJsonObject>
 #include <QTimer>
 #include <QUrl>
 
@@ -24,9 +26,6 @@
 #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/rapidjson/include/rapidjson/document.h"
-#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h"
-#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
 #include "rosbridge/rosbridge.h"
 
 QGC_LOGGING_CATEGORY(NemoInterfaceLog, "NemoInterfaceLog")
@@ -44,8 +43,8 @@ using ROSBridgePtr = std::shared_ptr<Rosbridge>;
 typedef ros_bridge::messages::nemo_msgs::tile::GenericTile<QGeoCoordinate,
                                                            QList>
     Tile;
-typedef std::map<std::int64_t, std::shared_ptr<Tile>> TileMap;
-typedef std::map<std::int64_t, std::shared_ptr<const Tile>> TileMapConst;
+typedef std::map<QString, std::shared_ptr<Tile>> TileMap;
+typedef std::map<QString, std::shared_ptr<const Tile>> TileMapConst;
 typedef ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat Heartbeat;
 typedef nemo_interface::TaskDispatcher Dispatcher;
 typedef nemo_interface::FutureWatcher<QVariant, std::shared_future>
@@ -599,19 +598,10 @@ void NemoInterface::Impl::_doTopicServiceSetup() {
   using namespace ros_bridge::messages;
 
   // Subscribe nemo progress.
-  this->_pRosbridge->subscribeTopic(progressTopic, [this](
-                                                       const QJsonObject &o) {
-    QString msg = QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact);
-
-    // parse in_message
-    rapidjson::Document d;
-    d.Parse(msg.toUtf8());
-    if (!d.HasParseError()) {
-      if (d.HasMember("msg") && d["msg"].IsObject()) {
-
-        // create obj from json
+  this->_pRosbridge->subscribeTopic(
+      progressTopic, [this](const QJsonObject &o) {
         nemo_msgs::progress_array::ProgressArray progressArray;
-        if (nemo_msgs::progress_array::fromJson(d["msg"], progressArray)) {
+        if (nemo_msgs::progress_array::fromJson(o, progressArray)) {
 
           // correct range errors of progress
           for (auto &lp : progressArray.progress_array()) {
@@ -645,32 +635,15 @@ void NemoInterface::Impl::_doTopicServiceSetup() {
         } else {
           qCWarning(NemoInterfaceLog) << "/nemo/progress not able to "
                                          "create ProgressArray form json: "
-                                      << msg;
+                                      << o;
         }
-      } else {
-        qCWarning(NemoInterfaceLog)
-            << "/nemo/progress no \"msg\" key or wrong type: " << msg;
-      }
-    } else {
-      qCWarning(NemoInterfaceLog) << "/nemo/progress message parse error ("
-                                  << d.GetParseError() << "): " << msg;
-    }
-  });
+      });
 
   // Subscribe heartbeat msg.
-  this->_pRosbridge->subscribeTopic(heartbeatTopic, [this](
-                                                        const QJsonObject &o) {
-    QString msg = QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact);
-
-    // parse in_message
-    rapidjson::Document d;
-    d.Parse(msg.toUtf8());
-    if (!d.HasParseError()) {
-      if (d.HasMember("msg") && d["msg"].IsObject()) {
-
-        // create obj from json
+  this->_pRosbridge->subscribeTopic(
+      heartbeatTopic, [this](const QJsonObject &o) {
         nemo_msgs::heartbeat::Heartbeat heartbeat;
-        if (nemo_msgs::heartbeat::fromJson(d["msg"], heartbeat)) {
+        if (nemo_msgs::heartbeat::fromJson(o, heartbeat)) {
           std::promise<bool> promise;
           auto future = promise.get_future();
           bool value = QMetaObject::invokeMethod(
@@ -683,17 +656,9 @@ void NemoInterface::Impl::_doTopicServiceSetup() {
         } else {
           qCWarning(NemoInterfaceLog) << "/nemo/heartbeat not able to "
                                          "create Heartbeat form json: "
-                                      << msg;
+                                      << o;
         }
-      } else {
-        qCWarning(NemoInterfaceLog)
-            << "/nemo/heartbeat no \"msg\" key or wrong type: " << msg;
-      }
-    } else {
-      qCWarning(NemoInterfaceLog) << "/nemo/heartbeat message parse error ("
-                                  << d.GetParseError() << "): " << msg;
-    }
-  });
+      });
 }
 
 void NemoInterface::Impl::_trySynchronize() {
@@ -701,6 +666,11 @@ void NemoInterface::Impl::_trySynchronize() {
        this->_state == STATE::USER_SYNC) &&
       !_isSynchronized()) {
 
+    if (!_dispatcher.idle()) {
+      QTimer::singleShot(5000, [this] { this->_trySynchronize(); });
+      return;
+    }
+
     qCWarning(NemoInterfaceLog) << "trying to synchronize";
 
     this->_setState(STATE::SYS_SYNC);
@@ -711,8 +681,6 @@ void NemoInterface::Impl::_trySynchronize() {
         std::bind(&Impl::_callClearTiles, this));
 
     // dispatch command.
-    _dispatcher.clear();
-    _dispatcher.stop();
     Q_ASSERT(_dispatcher.pendingTasks() == 0);
     auto ret = _dispatcher.dispatch(std::move(pTask));
     auto clearFuture = ret.share();
@@ -775,7 +743,6 @@ void NemoInterface::Impl::_doAction() {
   case STATE::START_BRIDGE:
     this->_pRosbridge->start();
     break;
-    break;
   case STATE::WEBSOCKET_DETECTED:
     resetDone = false;
     this->_setState(STATE::TRY_TOPIC_SERVICE_SETUP);
@@ -814,62 +781,38 @@ QVariant NemoInterface::Impl::_callAddTiles(
   this->_lastCall = CALL_NAME::ADD_TILES;
 
   // create json object
-  rapidjson::Document request(rapidjson::kObjectType);
-  auto &allocator = request.GetAllocator();
-  rapidjson::Value jsonTileArray(rapidjson::kArrayType);
+  QJsonArray jsonTileArray;
   for (const auto &tile : *pTileArray) {
     using namespace ros_bridge::messages;
-    rapidjson::Value jsonTile(rapidjson::kObjectType);
-    if (!nemo_msgs::tile::toJson(*tile, jsonTile, allocator)) {
+    QJsonObject jsonTile;
+    if (!nemo_msgs::tile::toJson(*tile, jsonTile)) {
       qCDebug(NemoInterfaceLog)
           << "addTiles(): not able to create json object: tile id: "
           << tile->id() << " progress: " << tile->progress()
           << " points: " << tile->tile();
     }
-    jsonTileArray.PushBack(jsonTile, allocator);
+    jsonTileArray.append(std::move(jsonTile));
   } // for
 
-  rapidjson::Value tileKey("in_tile_array");
-  request.AddMember(tileKey, jsonTileArray, allocator);
-
-  rapidjson::StringBuffer buffer;
-  rapidjson::Writer<rapidjson::StringBuffer> writer(buffer);
-  request.Accept(writer);
-  QJsonDocument req = QJsonDocument::fromJson(buffer.GetString());
+  QJsonObject req;
+  req["in_tile_array"] = std::move(jsonTileArray);
 
   // create response handler.
   auto promise_response = std::make_shared<std::promise<bool>>();
   auto future_response = promise_response->get_future();
   auto responseHandler = [promise_response](const QJsonObject &o) mutable {
     // check if transaction was successfull
-    QString msg = QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact);
-    rapidjson::Document d;
-    d.Parse(msg.toUtf8());
-    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;
-          promise_response->set_value(false);
-        }
-      } else {
-        qCWarning(NemoInterfaceLog)
-            << "/nemo/add_tiles no \"values\" key or wrong type: " << msg;
-        promise_response->set_value(false);
-      }
+    if (o.contains("success") && o["success"].isBool()) {
+      promise_response->set_value(o["success"].toBool());
     } else {
-      qCWarning(NemoInterfaceLog) << "/nemo/add_tiles message parse error ("
-                                  << d.GetParseError() << "): " << msg;
+      qCWarning(NemoInterfaceLog)
+          << "/nemo/add_tiles no \"success\" key or wrong type: " << o;
       promise_response->set_value(false);
     }
   };
 
   // call service.
-  this->_pRosbridge->callService("/nemo/add_tiles", responseHandler,
-                                 req.object());
+  this->_pRosbridge->callService("/nemo/add_tiles", responseHandler, req);
 
   // wait for response.
   auto tStart = hrc::now();
@@ -915,20 +858,13 @@ NemoInterface::Impl::_callRemoveTiles(std::shared_ptr<IDArray> pIdArray) {
   this->_lastCall = CALL_NAME::REMOVE_TILES;
 
   // create json object
-  rapidjson::Document request(rapidjson::kObjectType);
-  auto &allocator = request.GetAllocator();
-  rapidjson::Value jsonIdArray(rapidjson::kArrayType);
+  QJsonArray jsonIdArray;
   for (const auto id : *pIdArray) {
     using namespace ros_bridge::messages;
-    jsonIdArray.PushBack(rapidjson::Value(id), allocator);
+    jsonIdArray.append(id);
   } // for
-  rapidjson::Value tileKey("ids");
-  request.AddMember(tileKey, jsonIdArray, allocator);
-
-  rapidjson::StringBuffer buffer;
-  rapidjson::Writer<rapidjson::StringBuffer> writer(buffer);
-  request.Accept(writer);
-  QJsonDocument req = QJsonDocument::fromJson(buffer.GetString());
+  QJsonObject req;
+  req["ids"] = std::move(jsonIdArray);
 
   // create response handler.
   auto promise_response = std::make_shared<std::promise<bool>>();
@@ -936,33 +872,17 @@ NemoInterface::Impl::_callRemoveTiles(std::shared_ptr<IDArray> pIdArray) {
   auto responseHandler = [promise_response](const QJsonObject &o) mutable {
     // check if transaction was successfull
     QString msg = QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact);
-    rapidjson::Document d;
-    d.Parse(msg.toUtf8());
-    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;
-          promise_response->set_value(false);
-        }
-      } else {
-        qCWarning(NemoInterfaceLog)
-            << "/nemo/remove_tiles no \"values\" key or wrong type: " << msg;
-        promise_response->set_value(false);
-      }
+    if (o.contains("success") && o["success"].isBool()) {
+      promise_response->set_value(o["success"].toBool());
     } else {
-      qCWarning(NemoInterfaceLog) << "/nemo/remove_tiles message parse error ("
-                                  << d.GetParseError() << "): " << msg;
+      qCWarning(NemoInterfaceLog)
+          << "/nemo/remove_tiles no \"success\" key or wrong type: " << msg;
       promise_response->set_value(false);
     }
   };
 
   // call service.
-  this->_pRosbridge->callService("/nemo/remove_tiles", responseHandler,
-                                 req.object());
+  this->_pRosbridge->callService("/nemo/remove_tiles", responseHandler, req);
 
   // wait for response.
   auto tStart = hrc::now();
@@ -1011,22 +931,7 @@ QVariant NemoInterface::Impl::_callClearTiles() {
   auto future_response = promise_response->get_future();
   auto responseHandler = [promise_response](const QJsonObject &o) mutable {
     // check if transaction was successfull
-    QString msg = QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact);
-    rapidjson::Document d;
-    d.Parse(msg.toUtf8());
-    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;
-        promise_response->set_value(false);
-      }
-    } else {
-      qCWarning(NemoInterfaceLog) << "/nemo/clear_tiles message parse error ("
-                                  << d.GetParseError() << "): " << msg;
-      promise_response->set_value(false);
-    }
+    promise_response->set_value(true);
   };
 
   // call service.
@@ -1075,20 +980,13 @@ NemoInterface::Impl::_callGetProgress(std::shared_ptr<IDArray> pIdArray) {
   this->_lastCall = CALL_NAME::GET_PROGRESS;
 
   // create json object
-  rapidjson::Document request(rapidjson::kObjectType);
-  auto &allocator = request.GetAllocator();
-  rapidjson::Value jsonIdArray(rapidjson::kArrayType);
+  QJsonArray jsonIdArray;
   for (const auto id : *pIdArray) {
     using namespace ros_bridge::messages;
-    jsonIdArray.PushBack(rapidjson::Value(id), allocator);
+    jsonIdArray.append(id);
   } // for
-  rapidjson::Value tileKey("ids");
-  request.AddMember(tileKey, jsonIdArray, allocator);
-
-  rapidjson::StringBuffer buffer;
-  rapidjson::Writer<rapidjson::StringBuffer> writer(buffer);
-  request.Accept(writer);
-  QJsonDocument req = QJsonDocument::fromJson(buffer.GetString());
+  QJsonObject req;
+  req["ids"] = std::move(jsonIdArray);
 
   // create response handler.
   typedef std::shared_ptr<ProgressArray> ResponseType;
@@ -1096,40 +994,23 @@ NemoInterface::Impl::_callGetProgress(std::shared_ptr<IDArray> pIdArray) {
   auto future_response = promise_response->get_future();
   auto responseHandler = [promise_response](const QJsonObject &o) mutable {
     // check if transaction was successfull
-    QString msg = QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact);
-    rapidjson::Document d;
-    d.Parse(msg.toUtf8());
-    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<ProgressArray>();
-          *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;
-        promise_response->set_value(nullptr);
-      }
+    ros_bridge::messages::nemo_msgs::progress_array::ProgressArray
+        progressArrayMsg;
+    if (ros_bridge::messages::nemo_msgs::progress_array::fromJson(
+            o, progressArrayMsg)) {
+      auto pArray = std::make_shared<ProgressArray>();
+      *pArray = std::move(progressArrayMsg.progress_array());
+      promise_response->set_value(pArray);
     } else {
-      qCWarning(NemoInterfaceLog) << "/nemo/get_progress message parse error ("
-                                  << d.GetParseError() << "): " << msg;
+      qCWarning(NemoInterfaceLog)
+          << "/nemo/get_progress error while creating ProgressArray "
+             "from json.";
       promise_response->set_value(nullptr);
     }
   };
 
   // call service.
-  this->_pRosbridge->callService("/nemo/get_progress", responseHandler,
-                                 req.object());
+  this->_pRosbridge->callService("/nemo/get_progress", responseHandler, req);
 
   // wait for response.
   auto tStart = hrc::now();
@@ -1179,34 +1060,18 @@ QVariant NemoInterface::Impl::_callGetAllProgress() {
   auto future_response = promise_response->get_future();
   auto responseHandler = [promise_response](const QJsonObject &o) mutable {
     // check if transaction was successfull
-    QString msg = QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact);
-    rapidjson::Document d;
-    d.Parse(msg.toUtf8());
-    if (!d.HasParseError()) {
-      if (d.HasMember("values") && d["values"].IsObject()) {
-        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<ProgressArray>();
-          *pArray = std::move(progressArrayMsg.progress_array());
-          promise_response->set_value(pArray);
-        } else {
-          qCWarning(NemoInterfaceLog)
-              << "/nemo/all_get_progress error while creating ProgressArray "
-                 "from json.";
-          promise_response->set_value(nullptr);
-        }
-      } else {
-        qCWarning(NemoInterfaceLog)
-            << "/nemo/all_get_progress no \"values\" key or wrong type: "
-            << msg;
-        promise_response->set_value(nullptr);
-      }
+    ros_bridge::messages::nemo_msgs::progress_array::ProgressArray
+        progressArrayMsg;
+    if (ros_bridge::messages::nemo_msgs::progress_array::fromJson(
+            o, progressArrayMsg)) {
+      auto pArray = std::make_shared<ProgressArray>();
+      *pArray = std::move(progressArrayMsg.progress_array());
+      promise_response->set_value(pArray);
     } else {
       qCWarning(NemoInterfaceLog)
-          << "/nemo/all_get_progress message parse error (" << d.GetParseError()
-          << "): " << msg;
+          << "/nemo/get_all_progress error while creating ProgressArray "
+             "from json. msg: "
+          << o;
       promise_response->set_value(nullptr);
     }
   };

src/MeasurementComplexItem/geometry/MeasurementArea.cc

@@ -19,6 +19,8 @@
 #define MAX_TILES 1000
 #endif
 
+QString randomId();
+
 using namespace geometry;
 namespace trans = bg::strategy::transform;
 
@@ -261,7 +263,7 @@ bool MeasurementArea::loadFromJson(const QJsonObject &json,
 
           // find unique id
           if (it != _indexMap.end()) {
-            auto newId = tile->id() + 1;
+            auto newId = MeasurementTile::randomId();
             constexpr long counterMax = 1e6;
             unsigned long counter = 0;
             for (; counter <= counterMax; ++counter) {
@@ -269,7 +271,7 @@ bool MeasurementArea::loadFromJson(const QJsonObject &json,
               if (it == _indexMap.end()) {
                 break;
               } else {
-                ++newId;
+                newId = MeasurementTile::randomId();
               }
             }
 
@@ -431,15 +433,11 @@ void MeasurementArea::doUpdate() {
           // Convert to geo system.
           for (const auto &t : tilesENU) {
             auto geoTile = new MeasurementTile(pData.get());
-            std::size_t hashValue = 0;
-            std::hash<QGeoCoordinate> hashFun;
             for (const auto &v : t.outer()) {
               QGeoCoordinate geoVertex;
               fromENU(origin, v, geoVertex);
               geoTile->push_back(geoVertex);
-              hashValue ^= hashFun(geoVertex);
             }
-            geoTile->setId(std::int64_t(hashValue));
             pData->append(geoTile);
           }
         }
@@ -504,7 +502,7 @@ void MeasurementArea::storeTiles() {
 
       // find unique id
       if (it != _indexMap.end()) {
-        auto newId = tile->id() + 1;
+        auto newId = MeasurementTile::randomId();
         constexpr long counterMax = 1e6;
         unsigned long counter = 0;
         for (; counter <= counterMax; ++counter) {
@@ -512,7 +510,7 @@ void MeasurementArea::storeTiles() {
           if (it == _indexMap.end()) {
             break;
           } else {
-            ++newId;
+            newId = MeasurementTile::randomId();
           }
         }
 
@@ -710,3 +708,14 @@ bool getTiles(const FPolygon &area, Length tileHeight, Length tileWidth,
 
   return true;
 }
+
+QString randomId() {
+  std::srand(std::time(nullptr));
+  std::int64_t r = 0;
+
+  for (int i = 0; i < 10; ++i) {
+    r ^= std::rand();
+  }
+
+  return QString::number(r);
+}

src/MeasurementComplexItem/geometry/MeasurementArea.h

@@ -107,7 +107,7 @@ private: