temp

qgroundcontrol.pro

@@ -453,8 +453,9 @@ HEADERS += \
     src/MeasurementComplexItem/geometry/TileDiff.h \
     src/MeasurementComplexItem/geometry/geometry.h \
     src/MeasurementComplexItem/HashFunctions.h \
+    src/MeasurementComplexItem/nemo_interface/Task.h \
+    src/MeasurementComplexItem/nemo_interface/tileHelper.h \
     src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h \
-    src/MeasurementComplexItem/nemo_interface/Command.h \
     src/MeasurementComplexItem/nemo_interface/CommandDispatcher.h \
     src/MeasurementComplexItem/nemo_interface/MeasurementTile.h \
     src/QmlControls/QmlUnitsConversion.h \
@@ -496,11 +497,6 @@ HEADERS += \
     src/MeasurementComplexItem/geometry/GeoPoint3D.h \
     src/MeasurementComplexItem/NemoInterface.h \
     src/MeasurementComplexItem/nemo_interface/QNemoHeartbeat.h \
-    src/MeasurementComplexItem/nemo_interface/QNemoProgress.h \
-    src/MeasurementComplexItem/nemo_interface/QNemoProgress.h \
-    src/MeasurementComplexItem/nemo_interface/SnakeTileLocal.h \
-    src/MeasurementComplexItem/nemo_interface/SnakeTiles.h \
-    src/MeasurementComplexItem/nemo_interface/SnakeTilesLocal.h \
     src/MeasurementComplexItem/call_once.h \
     src/api/QGCCorePlugin.h \
     src/api/QGCOptions.h \
@@ -513,10 +509,6 @@ HEADERS += \
     src/comm/ros_bridge/include/com_private.h \
     src/comm/ros_bridge/include/message_traits.h \
     src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.h \
-    src/comm/ros_bridge/include/messages/geometry_msgs/point32.h \
-    src/comm/ros_bridge/include/messages/geometry_msgs/polygon.h \
-    src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.h \
-    src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h \
     src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h \
     src/comm/ros_bridge/include/messages/nemo_msgs/tile.h \
     src/comm/ros_bridge/include/messages/std_msgs/header.h \
@@ -537,9 +529,9 @@ SOURCES += \
     src/MeasurementComplexItem/geometry/SafeArea.cc \
     src/MeasurementComplexItem/geometry/geometry.cpp \
     src/MeasurementComplexItem/HashFunctions.cpp \
-    src/MeasurementComplexItem/nemo_interface/Command.cpp \
     src/MeasurementComplexItem/nemo_interface/CommandDispatcher.cpp \
     src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp \
+    src/MeasurementComplexItem/nemo_interface/Task.cpp \
     src/Vehicle/VehicleEscStatusFactGroup.cc \
     src/MeasurementComplexItem/AreaData.cc \
     src/api/QGCCorePlugin.cc \
@@ -555,15 +547,10 @@ SOURCES += \
     src/MeasurementComplexItem/geometry/clipper/clipper.cpp \
     src/MeasurementComplexItem/geometry/GeoPoint3D.cpp \
     src/MeasurementComplexItem/NemoInterface.cpp \
-    src/MeasurementComplexItem/nemo_interface/QNemoProgress.cc \
     src/comm/QmlObjectListHelper.cpp \
     src/comm/ros_bridge/include/RosBridgeClient.cpp \
     src/comm/ros_bridge/include/com_private.cpp \
     src/comm/ros_bridge/include/messages/geographic_msgs/geopoint.cpp \
-    src/comm/ros_bridge/include/messages/geometry_msgs/point32.cpp \
-    src/comm/ros_bridge/include/messages/geometry_msgs/polygon.cpp \
-    src/comm/ros_bridge/include/messages/geometry_msgs/polygon_stamped.cpp \
-    src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.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/tile.cpp \

src/MeasurementComplexItem/HashFunctions.h

@@ -11,7 +11,7 @@ namespace std {
 template <> struct hash<QGeoCoordinate> {
   std::size_t operator()(const QGeoCoordinate &c) {
     hash<double> h;
-    return h(c.latitude()) ^ h(c.longitude()) ^ h(c.altitude());
+    return h(c.latitude()) ^ (h(c.longitude()) << 1) ^ (h(c.altitude()) << 2);
   }
 };
 

src/MeasurementComplexItem/IDArray.h

 #ifndef IDARRAY_H
 #define IDARRAY_H
-typedef QVector<long> IDArray;
+
+#include <QVector>
+
+typedef QVector<std::int64_t> IDArray;
 
 #endif // IDARRAY_H

src/MeasurementComplexItem/LogicalArray.h

 #ifndef LOGICALARRAY_H
 #define LOGICALARRAY_H
+
+#include <QVector>
+
 typedef QVector<bool> LogicalArray;
 
 #endif // LOGICALARRAY_H

src/MeasurementComplexItem/NemoInterface.cpp

 #include "NemoInterface.h"
-#include "nemo_interface/SnakeTilesLocal.h"
 
 #include "QGCApplication.h"
 #include "QGCLoggingCategory.h"
@@ -15,14 +14,14 @@
 #include "GenericSingelton.h"
 #include "geometry/MeasurementArea.h"
 #include "geometry/geometry.h"
+#include "nemo_interface/CommandDispatcher.h"
 #include "nemo_interface/MeasurementTile.h"
 #include "nemo_interface/QNemoHeartbeat.h"
-#include "nemo_interface/QNemoProgress.h"
 
+#include "ros_bridge/include/RosBridgeClient.h"
 #include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
-#include "ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h"
 #include "ros_bridge/include/messages/nemo_msgs/heartbeat.h"
-#include "ros_bridge/include/messages/nemo_msgs/progress.h"
+#include "ros_bridge/include/messages/nemo_msgs/tile.h"
 #include "ros_bridge/include/ros_bridge.h"
 #include "ros_bridge/rapidjson/include/rapidjson/document.h"
 #include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h"
@@ -30,38 +29,35 @@
 
 QGC_LOGGING_CATEGORY(NemoInterfaceLog, "NemoInterfaceLog")
 
-#define EVENT_TIMER_INTERVAL 100 // ms
-auto constexpr static timeoutInterval = std::chrono::milliseconds(3000);
+#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;
 
-using ROSBridgePtr = std::unique_ptr<ros_bridge::ROSBridge>;
-using JsonDocUPtr = ros_bridge::com_private::JsonDocUPtr;
+using ROSBridgePtr = std::shared_ptr<RosbridgeWsClient>;
 using UniqueLock = std::unique_lock<std::shared_timed_mutex>;
 using SharedLock = std::shared_lock<std::shared_timed_mutex>;
-using JsonDocUPtr = ros_bridge::com_private::JsonDocUPtr;
 
 class NemoInterface::Impl {
-public:
   enum class STATE {
     STOPPED,
-    RUNNING,
+    START_BRIDGE,
     WEBSOCKET_DETECTED,
-    HEARTBEAT_DETECTED,
     TRY_TOPIC_SERVICE_SETUP,
     READY,
-    SYNCHRONIZING,
-    TIMEOUT,
-    INVALID_HEARTBEAT
-  }
+    TIMEOUT
+  };
 
+public:
   Impl(NemoInterface *p);
+  ~Impl();
 
   void start();
   void stop();
 
   // Tile editing.
   // 	Functions that require communication to device.
-  void addTiles(const TilePtrArray &tileArray);
-  void addTiles(const TileArray &tileArray);
+  std::future<QVariant> addTiles(const TileArray &tileArray);
   void removeTiles(const IDArray &idArray);
   void clearTiles();
 
@@ -77,42 +73,53 @@ public:
   ProgressArray getProgress(const IDArray &idArray);
 
   NemoInterface::STATUS status();
-  bool running();
+  bool running(); // thread safe
+  bool ready();   // thread safe
+
+  const QString &infoString();
+  const QString &warningString();
 
-private slots:
   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<std::chrono::high_resolution_clock> TimePoint;
   typedef std::map<long, MeasurementTile> TileMap;
   typedef ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat Heartbeat;
+  typedef nemo_interface::CommandDispatcher Dispatcher;
 
-  void doTopicServiceSetup();
-  void loop();
+  void _doTopicServiceSetup();
+  void _doAction(); // does action according to state
 
-  bool _setState(STATE s);
+  bool _setState(STATE s); // not thread safe
+  static bool _ready(STATE s);
   static bool _running(STATE s);
   static void _translate(STATE state, NemoInterface::STATUS &status);
   static void _translate(Heartbeat hb, STATE &state);
 
-  TimePoint nextTimeout;
-  mutable std::shared_timed_mutex timeoutMutex;
-
-  STATE _state;
+  std::atomic<STATE> _state;
   ROSBridgePtr _pRosBridge;
   TileMap _tileMap;
   NemoInterface *_parent;
+  Dispatcher _dispatcher;
+  QString _infoString;
+  QString _warningString;
+  QTimer _timeoutTimer;
+  QTimer _connectionTimer;
+  QNemoHeartbeat _lastHeartbeat;
 };
 
 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::HEARTBEAT_DETECTED, "Heartbeat Detected"),
+    std::make_pair<NemoInterface::STATUS, QString>(NemoInterface::STATUS::READY,
+                                                   "Ready"),
     std::make_pair<NemoInterface::STATUS, QString>(
         NemoInterface::STATUS::TIMEOUT, "Timeout"),
     std::make_pair<NemoInterface::STATUS, QString>(
@@ -121,8 +128,7 @@ static StatusMap statusMap{
         NemoInterface::STATUS::WEBSOCKET_DETECTED, "Websocket Detected")};
 
 NemoInterface::Impl::Impl(NemoInterface *p)
-    : nextTimeout(TimePoint::max()), status_(STATUS::NOT_CONNECTED),
-      running_(false), topicServiceSetupDone(false), _parent(p) {
+    : _state(STATE::STOPPED), _parent(p) {
 
   // ROS Bridge.
   WimaSettings *wimaSettings =
@@ -130,37 +136,151 @@ NemoInterface::Impl::Impl(NemoInterface *p)
   auto connectionStringFact = wimaSettings->rosbridgeConnectionString();
   auto setConnectionString = [connectionStringFact, this] {
     auto connectionString = connectionStringFact->rawValue().toString();
-    if (ros_bridge::isValidConnectionString(
-            connectionString.toLocal8Bit().data())) {
+    if (is_valid_port_path(connectionString.toLocal8Bit().data())) {
     } else {
       qgcApp()->warningMessageBoxOnMainThread(
           "Nemo Interface",
           "Websocket connection string possibly invalid: " + connectionString +
               ". Trying to connect anyways.");
     }
-    this->_pRosBridge.reset(
-        new ros_bridge::ROSBridge(connectionString.toLocal8Bit().data()));
+
+    if (this->_pRosBridge) {
+      this->_pRosBridge->reset();
+    }
+    this->_pRosBridge = std::make_shared<RosbridgeWsClient>(
+        connectionString.toLocal8Bit().data());
+    this->_pRosBridge->reset();
+    qCritical() << "NemoInterface: add reset code here";
   };
   connect(connectionStringFact, &SettingsFact::rawValueChanged,
           setConnectionString);
   setConnectionString();
 
-  // Periodic.
-  connect(&this->loopTimer, &QTimer::timeout, [this] { this->loop(); });
-  this->loopTimer.start(EVENT_TIMER_INTERVAL);
+  // Heartbeat timeout.
+  connect(&this->_timeoutTimer, &QTimer::timeout,
+          [this] { this->_setState(STATE::TIMEOUT); });
+
+  // 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->_setState(STATE::WEBSOCKET_DETECTED);
+        this->_doAction();
+      }
+    } else {
+      if (this->_state == STATE::TRY_TOPIC_SERVICE_SETUP ||
+          this->_state == STATE::READY) {
+        this->_setState(STATE::TIMEOUT);
+        this->_doAction();
+      }
+    }
+  });
 }
 
+NemoInterface::Impl::~Impl() {}
+
 void NemoInterface::Impl::start() {
-  this->running_ = true;
-  emit this->_parent->runningChanged();
+  if (!running()) {
+    this->_setState(STATE::START_BRIDGE);
+    this->_doAction();
+  }
 }
 
 void NemoInterface::Impl::stop() {
-  this->running_ = false;
-  emit this->_parent->runningChanged();
+  if (running()) {
+    this->_setState(STATE::STOPPED);
+    this->_connectionTimer.stop();
+    this->_doAction();
+  }
 }
 
-void NemoInterface::Impl::addTiles(const TilePtrArray &tileArray) {}
+std::future<QVariant>
+NemoInterface::Impl::addTiles(const TileArray &tileArray) {
+  using namespace nemo_interface;
+
+  if (this->ready()) {
+
+    // create command.
+    auto pRosBridge = this->_pRosBridge;
+    auto pDispatcher = &this->_dispatcher;
+    Task sendTilesCommand([pRosBridge, tileArray, pDispatcher,
+                           this](std::promise<QVariant> 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<std::promise<void>>();
+      auto future_response = promise_response->get_future();
+      auto responseHandler =
+          [promise_response](
+              std::shared_ptr<WsClient::Connection> connection,
+              std::shared_ptr<WsClient::InMessage> 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());
+
+      if (abort) {
+        qCWarning(NemoInterfaceLog) << "Websocket not responding to request.";
+        promise.set_value(QVariant(false));
+        return;
+      }
+
+      qCritical() << "addTiles(): ToDo: add return value checking here.";
+
+      // update local tiles
+      QMetaObject::invokeMethod(
+          this->_parent, std::bind(&Impl::_addTilesLocal, this, tileArray));
+
+      // return success
+      promise.set_value(QVariant(true));
+      return;
+    }); // sendTilesCommand
+
+    // dispatch command and return.
+    auto ret = _dispatcher.dispatch(sendTilesCommand);
+    return ret;
+  } else {
+    std::promise<QVariant> p;
+    p.set_value(QVariant(false));
+    return p.get_future();
+  }
+}
 
 TileArray NemoInterface::Impl::getTiles(const IDArray &idArray) {
   TileArray tileArray;
@@ -215,7 +335,7 @@ ProgressArray NemoInterface::Impl::getProgress(const IDArray &idArray) {
 
   for (const auto &id : idArray) {
     const auto it = _tileMap.find(id);
-    if (id != _tileMap.end()) {
+    if (it != _tileMap.end()) {
       progressArray.append(TaggedProgress{it->first, it->second.progress()});
     }
   }
@@ -223,78 +343,19 @@ ProgressArray NemoInterface::Impl::getProgress(const IDArray &idArray) {
   return progressArray;
 }
 
-void NemoInterface::Impl::setTileData(const TileData &tileData) {
-  this->tileData = tileData;
-  if (tileData.tiles.count() > 0) {
-    std::lock(this->ENUOriginMutex, this->tilesENUMutex);
-    UniqueLock lk1(this->ENUOriginMutex, std::adopt_lock);
-    UniqueLock lk2(this->tilesENUMutex, std::adopt_lock);
-
-    const auto *obj = tileData.tiles[0];
-    const auto *tile = qobject_cast<const MeasurementTile *>(obj);
-    if (tile != nullptr) {
-      if (tile->coordinateList().size() > 0) {
-        if (tile->coordinateList().first().isValid()) {
-          this->ENUOrigin = tile->coordinateList().first();
-          const auto &origin = this->ENUOrigin;
-          this->tilesENU.polygons().clear();
-          for (int i = 0; i < tileData.tiles.count(); ++i) {
-            obj = tileData.tiles[i];
-            tile = qobject_cast<const MeasurementTile *>(obj);
-            if (tile != nullptr) {
-              SnakeTileLocal tileENU;
-              geometry::areaToEnu(origin, tile->coordinateList(),
-                                  tileENU.path());
-              this->tilesENU.polygons().push_back(std::move(tileENU));
-            } else {
-              qCDebug(NemoInterfaceLog) << "Impl::setTileData(): nullptr.";
-              break;
-            }
-          }
-        } else {
-          qCDebug(NemoInterfaceLog) << "Impl::setTileData(): Origin invalid.";
-        }
-      } else {
-        qCDebug(NemoInterfaceLog) << "Impl::setTileData(): tile empty.";
-      }
-    }
-  } else {
-    this->tileData.clear();
-
-    std::lock(this->ENUOriginMutex, this->tilesENUMutex);
-    UniqueLock lk1(this->ENUOriginMutex, std::adopt_lock);
-    UniqueLock lk2(this->tilesENUMutex, std::adopt_lock);
-    this->ENUOrigin = QGeoCoordinate(0, 0, 0);
-    this->tilesENU = SnakeTilesLocal();
-  }
-}
-
-bool NemoInterface::Impl::hasTileData(const TileData &tileData) const {
-  return this->tileData == tileData;
+NemoInterface::STATUS NemoInterface::Impl::status() {
+  NemoInterface::STATUS status;
+  _translate(this->_state, status);
+  return status;
 }
 
-void NemoInterface::Impl::publishTileData() {
-  std::lock(this->ENUOriginMutex, this->tilesENUMutex);
-  UniqueLock lk1(this->ENUOriginMutex, std::adopt_lock);
-  UniqueLock lk2(this->tilesENUMutex, std::adopt_lock);
+bool NemoInterface::Impl::running() { return _running(this->_state); }
 
-  if (this->tilesENU.polygons().size() > 0 && this->running_ &&
-      this->topicServiceSetupDone) {
-    this->publishENUOrigin();
-    this->publishTilesENU();
-  }
-}
+bool NemoInterface::Impl::ready() { return _ready(this->_state.load()); }
 
-NemoInterface::STATUS NemoInterface::Impl::status() {
-  return _translate(this->_state);
-}
+const QString &NemoInterface::Impl::infoString() { return _infoString; }
 
-QVector<int> NemoInterface::Impl::progress() {
-  SharedLock lk(this->progressMutex);
-  return this->qProgress.progress();
-}
-
-bool NemoInterface::Impl::running() { return _running(this->_state); }
+const QString &NemoInterface::Impl::warningString() { return _warningString; }
 
 void NemoInterface::Impl::_addTilesLocal(const TileArray &tileArray) {
   bool anyChanges = false;
@@ -363,133 +424,88 @@ void NemoInterface::Impl::_updateProgress(ProgressArray progressArray) {
   emit _parent->progressChanged(progressArray);
 }
 
-void NemoInterface::Impl::doTopicServiceSetup() {
+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::_setInfoString(const QString &info) {
+  if (_infoString != info) {
+    _infoString = info;
+    emit this->_parent->infoStringChanged();
+  }
+}
+
+void NemoInterface::Impl::_setWarningString(const QString &warning) {
+  if (_warningString != warning) {
+    _warningString = warning;
+    emit this->_parent->warningStringChanged();
+  }
+}
+
+void NemoInterface::Impl::_doTopicServiceSetup() {
   using namespace ros_bridge::messages;
 
   // Subscribe nemo progress.
+  const char *progressClient = "client:/nemo/progress";
+  this->_pRosBridge->addClient(progressClient);
   this->_pRosBridge->subscribe(
-      "/nemo/progress",
-      /* callback */ [this](JsonDocUPtr pDoc) {
-        std::lock(this->progressMutex, this->tilesENUMutex,
-                  this->ENUOriginMutex);
-        UniqueLock lk1(this->progressMutex, std::adopt_lock);
-        UniqueLock lk2(this->tilesENUMutex, std::adopt_lock);
-        UniqueLock lk3(this->ENUOriginMutex, std::adopt_lock);
-
-        int requiredSize = this->tilesENU.polygons().size();
-        auto &progressMsg = this->qProgress;
-        if (!nemo_msgs::progress::fromJson(*pDoc, progressMsg) ||
-            progressMsg.progress().size() !=
-                requiredSize) { // Some error occured.
-          progressMsg.progress().clear();
-          qgcApp()->informationMessageBoxOnMainThread(
-              "Nemo Interface", "Invalid progress message received.");
-        }
-        emit this->_parent->progressChanged();
-
-        lk1.unlock();
-        lk2.unlock();
-        lk3.unlock();
+      progressClient, "/nemo/progress",
+      [this](std::shared_ptr<WsClient::Connection> connection,
+             std::shared_ptr<WsClient::InMessage> in_message) {
+        qDebug() << "doTopicServiceSetup(): /nemo/progress: "
+                 << in_message->string().c_str();
+        qDebug() << "impl missing";
       });
-
-  // Subscribe /nemo/heartbeat.
+  // Subscribe heartbeat msg.
+  const char *heartbeatClient = "client:/nemo/heartbeat";
+  this->_pRosBridge->addClient(heartbeatClient);
   this->_pRosBridge->subscribe(
-      "/nemo/heartbeat",
-      /* callback */ [this](JsonDocUPtr pDoc) {
-        nemo_msgs::heartbeat::Heartbeat heartbeatMsg;
-        if (!nemo_msgs::heartbeat::fromJson(*pDoc, heartbeatMsg)) {
-          this->_setState(STATUS::INVALID_HEARTBEAT);
-        } else {
-          this->_setState(heartbeatToStatus(heartbeatMsg));
-        }
-        if (this->status_ == STATUS::INVALID_HEARTBEAT) {
-          UniqueLock lk(this->timeoutMutex);
-          this->nextTimeout = TimePoint::max();
-        } else if (this->status_ == STATUS::HEARTBEAT_DETECTED) {
-          UniqueLock lk(this->timeoutMutex);
-          this->nextTimeout =
-              std::chrono::high_resolution_clock::now() + timeoutInterval;
-        }
+      heartbeatClient, "/nemo/heartbeat",
+      [this](std::shared_ptr<WsClient::Connection> connection,
+             std::shared_ptr<WsClient::InMessage> in_message) {
+        qDebug() << "doTopicServiceSetup(): /nemo/heartbeat: "
+                 << in_message->string().c_str();
+        qDebug() << "impl missing";
       });
 }
 
-void NemoInterface::Impl::loop() {
+void NemoInterface::Impl::_doAction() {
   // Check ROS Bridge status and do setup if necessary.
-  if (this->running_) {
-    if (!this->_pRosBridge->isRunning()) {
-      this->_pRosBridge->start();
-      this->loop();
-    } else if (this->_pRosBridge->isRunning() &&
-               this->_pRosBridge->connected() && !this->topicServiceSetupDone) {
-      this->doTopicServiceSetup();
-      this->topicServiceSetupDone = true;
-
-      this->_setState(STATUS::WEBSOCKET_DETECTED);
-    } else if (this->_pRosBridge->isRunning() &&
-               !this->_pRosBridge->connected() && this->topicServiceSetupDone) {
+  switch (this->_state) {
+  case STATE::STOPPED:
+    if (this->_pRosBridge->running()) {
       this->_pRosBridge->reset();
-      this->_pRosBridge->start();
-      this->topicServiceSetupDone = false;
-
-      this->_setState(STATUS::TIMEOUT);
     }
-  } else if (this->_pRosBridge->isRunning()) {
-    this->_pRosBridge->reset();
-    this->topicServiceSetupDone = false;
-  }
-
-  // Check if heartbeat timeout occured.
-  if (this->running_ && this->topicServiceSetupDone) {
-    UniqueLock lk(this->timeoutMutex);
-    if (this->nextTimeout != TimePoint::max() &&
-        this->nextTimeout < std::chrono::high_resolution_clock::now()) {
-      lk.unlock();
-      if (this->_pRosBridge->isRunning() && this->_pRosBridge->connected()) {
-        this->_setState(STATUS::WEBSOCKET_DETECTED);
-      } else {
-        this->_setState(STATUS::TIMEOUT);
-      }
-    }
-  }
-}
+    break;
 
-NemoInterface::STATUS NemoInterface::Impl::heartbeatToStatus(
-    const ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat &hb) {
-  if (STATUS(hb.status()) == STATUS::HEARTBEAT_DETECTED)
-    return STATUS::HEARTBEAT_DETECTED;
-  else
-    return STATUS::INVALID_HEARTBEAT;
-}
-
-void NemoInterface::Impl::publishTilesENU() {
-  using namespace ros_bridge::messages;
-  JsonDocUPtr jSnakeTiles(
-      std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-  if (jsk_recognition_msgs::polygon_array::toJson(
-          this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator())) {
-    this->_pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
-  } else {
-    qCWarning(NemoInterfaceLog)
-        << "Impl::publishTilesENU: could not create json document.";
-  }
-}
-
-void NemoInterface::Impl::publishENUOrigin() {
-  using namespace ros_bridge::messages;
-  JsonDocUPtr jOrigin(
-      std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-  if (geographic_msgs::geo_point::toJson(this->ENUOrigin, *jOrigin,
-                                         jOrigin->GetAllocator())) {
-    this->_pRosBridge->publish(std::move(jOrigin), "/snake/origin");
-  } else {
-    qCWarning(NemoInterfaceLog)
-        << "Impl::publishENUOrigin: could not create json document.";
-  }
+  case STATE::START_BRIDGE:
+  case STATE::TIMEOUT:
+    this->_pRosBridge->reset();
+    this->_pRosBridge->run();
+    this->_connectionTimer.start(EVENT_TIMER_INTERVAL);
+    break;
+  case STATE::WEBSOCKET_DETECTED:
+    this->_setState(STATE::TRY_TOPIC_SERVICE_SETUP);
+    this->_doAction();
+    break;
+  case STATE::TRY_TOPIC_SERVICE_SETUP:
+    this->_doTopicServiceSetup();
+    this->_setState(STATE::READY);
+    break;
+  case STATE::READY:
+    break;
+  };
 }
 
 bool NemoInterface::Impl::_setState(STATE s) {
-  if (s != this->status_) {
-    this->status_ = s;
+  if (s != this->_state) {
+    this->_state = s;
     emit this->_parent->statusChanged();
     return true;
   } else {
@@ -497,6 +513,14 @@ bool NemoInterface::Impl::_setState(STATE s) {
   }
 }
 
+bool NemoInterface::Impl::_ready(NemoInterface::Impl::STATE s) {
+  return s == STATE::READY;
+}
+
+bool NemoInterface::Impl::_running(NemoInterface::Impl::STATE s) {
+  return s != STATE::STOPPED;
+}
+
 // ===============================================================
 // NemoInterface
 NemoInterface::NemoInterface()
@@ -515,16 +539,8 @@ void NemoInterface::start() { this->pImpl->start(); }
 
 void NemoInterface::stop() { this->pImpl->stop(); }
 
-void NemoInterface::addTiles(const NemoInterface::TilePtrArray &tileArray) {
-  this->pImpl -
-}
-
 void NemoInterface::addTiles(const TileArray &tileArray) {
-  TilePtrArray ptrArray;
-  for (const auto &tile : tileArray) {
-    ptrArray.append(&tile);
-  }
-  addTiles(ptrArray);
+  this->pImpl->addTiles(tileArray);
 }
 
 void NemoInterface::removeTiles(const IDArray &idArray) {
@@ -537,9 +553,7 @@ TileArray NemoInterface::getTiles(const IDArray &idArray) {
   return this->pImpl->getTiles(idArray);
 }
 
-TileArray NemoInterface::getAllTiles() {
-  return this->pImpl->getTiles(idArray);
-}
+TileArray NemoInterface::getAllTiles() { return this->pImpl->getAllTiles(); }
 
 LogicalArray NemoInterface::containsTiles(const IDArray &idArray) {
   return this->pImpl->containsTiles(idArray);
@@ -557,20 +571,6 @@ ProgressArray NemoInterface::getProgress(const IDArray &idArray) {
   return this->pImpl->getProgress(idArray);
 }
 
-void NemoInterface::publishTileData() { this->pImpl->publishTileData(); }
-
-void NemoInterface::requestProgress() {
-  qCWarning(NemoInterfaceLog) << "requestProgress(): dummy.";
-}
-
-void NemoInterface::setTileData(const TileData &tileData) {
-  this->pImpl->setTileData(tileData);
-}
-
-bool NemoInterface::hasTileData(const TileData &tileData) const {
-  return this->pImpl->hasTileData(tileData);
-}
-
 NemoInterface::STATUS NemoInterface::status() const {
   return this->pImpl->status();
 }
@@ -579,7 +579,11 @@ QString NemoInterface::statusString() const {
   return statusMap.at(this->pImpl->status());
 }
 
-QVector<int> NemoInterface::progress() const { return this->pImpl->progress(); }
+QString NemoInterface::infoString() const { return this->pImpl->infoString(); }
+
+QString NemoInterface::warningString() const {
+  return this->pImpl->warningString();
+}
 
 QString NemoInterface::editorQml() {
   return QStringLiteral("NemoInterface.qml");

src/MeasurementComplexItem/NemoInterface.h

@@ -37,6 +37,9 @@ public:
 
   Q_PROPERTY(STATUS status READ status NOTIFY statusChanged)
   Q_PROPERTY(QString statusString READ statusString NOTIFY statusChanged)
+  Q_PROPERTY(QString infoString READ infoString NOTIFY infoStringChanged)
+  Q_PROPERTY(
+      QString warningString READ warningString NOTIFY warningStringChanged)
   Q_PROPERTY(QString editorQml READ editorQml CONSTANT)
   Q_PROPERTY(bool running READ running NOTIFY runningChanged)
 
@@ -46,7 +49,6 @@ public:
   Q_INVOKABLE void stop();
 
   // Tile editing.
-  void addTiles(const TilePtrArray &tileArray);
   void addTiles(const TileArray &tileArray);
   void removeTiles(const IDArray &idArray);
   void clearTiles();
@@ -63,10 +65,14 @@ public:
   // Status.
   STATUS status() const;
   QString statusString() const;
+  QString infoString() const;
+  QString warningString() const;
   bool running();
 
 signals:
   void statusChanged();
+  void infoStringChanged();
+  void warningStringChanged();
   void progressChanged(const ProgressArray &progressArray);
   void tilesChanged();
   void runningChanged();

src/MeasurementComplexItem/TileArray.h

 #ifndef TILEARRAY_H
 #define TILEARRAY_H
+#include "MeasurementTile.h"
+
 typedef QVector<MeasurementTile> TileArray;
 
 #endif // TILEARRAY_H

src/MeasurementComplexItem/TilePtrArray.h

 #ifndef TILEPTRARRAY_H
 #define TILEPTRARRAY_H
+
+#include "MeasurementTile.h"
+
 typedef QVector<MeasurementTile *> TilePtrArray;
 
 #endif // TILEPTRARRAY_H

src/MeasurementComplexItem/nemo_interface/Command.cpp

-#include "Command.h"
-
-namespace nemo_interface {
-
-Command::Command(Functor onExec) : _onExec(onExec) {}
-
-QFuture<Command::ERROR> Command::exec() { return _onExec(); }
-
-} // namespace nemo_interface

src/MeasurementComplexItem/nemo_interface/Command.h

-#ifndef COMMAND_H
-#define COMMAND_H
-
-#include <QFuture>
-#include <functional>
-
-namespace nemo_interface {
-
-class Command {
-public:
-  enum class ERROR {
-    NO_ERROR,
-    NETWORK_TIMEOUT,
-    PARAMETER_ERROR,
-    UNEXPECTED_SERVER_RESPONSE
-  };
-
-  typedef QFuture<ERROR> ReturnType;
-  typedef std::function<ReturnType()> Functor;
-
-  Command(Functor onExec);
-
-  QFuture<ERROR> exec();
-
-private:
-  Functor _onExec;
-};
-} // namespace nemo_interface
-
-#endif // COMMAND_H

src/MeasurementComplexItem/nemo_interface/CommandDispatcher.cpp

 #include "CommandDispatcher.h"
 
-CommandDispatcher::CommandDispatcher()
-{
+namespace nemo_interface {
 
-}
+CommandDispatcher::CommandDispatcher() {}
+
+} // namespace nemo_interface

src/MeasurementComplexItem/nemo_interface/CommandDispatcher.h

 #ifndef COMMANDDISPATCHER_H
 #define COMMANDDISPATCHER_H
 
+#include <QThread>
+#include <QVariant>
 
-class CommandDispatcher
-{
+#include "Task.h"
+
+namespace nemo_interface {
+
+class CommandDispatcher : public QThread {
 public:
-    CommandDispatcher();
+  CommandDispatcher();
+
+  bool interruptionPoint(); // thread safe
+
+  std::future<QVariant> dispatch(const Task &c);     // thread safe
+  std::future<QVariant> 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<Task> _queue;
+  std::mutex _queueMutex;
 };
+} // namespace nemo_interface
 
 #endif // COMMANDDISPATCHER_H

src/MeasurementComplexItem/nemo_interface/MeasurementTile.cpp

-#include "MeasurementTile.h"
+#include "MeasurementTile.h"
 
 MeasurementTile::MeasurementTile(QObject *parent)
     : GeoArea(parent), _progress(0), _id(0) {
@@ -33,15 +33,17 @@ void MeasurementTile::push_back(const QGeoCoordinate &c) {
 
 void MeasurementTile::init() { this->setObjectName("Tile"); }
 
-uint64_t MeasurementTile::id() const { return _id; }
+int64_t MeasurementTile::id() const { return _id; }
 
-void MeasurementTile::setId(const uint64_t &id) {
+void MeasurementTile::setId(const int64_t &id) {
   if (_id != id) {
     _id = id;
     emit idChanged();
   }
 }
 
+QList<QGeoCoordinate> MeasurementTile::tile() const { return coordinateList(); }
+
 double MeasurementTile::progress() const { return _progress; }
 
 void MeasurementTile::setProgress(double progress) {

src/MeasurementComplexItem/nemo_interface/MeasurementTile.h

@@ -14,7 +14,6 @@ public:
   MeasurementTile &operator=(const MeasurementTile &other);
 
   Q_PROPERTY(double progress READ progress NOTIFY progressChanged)
-  Q_PROPERTY(long id READ id NOTIFY idChanged)
 
   virtual QString mapVisualQML() const override;
   virtual QString editorQML() const override;
@@ -25,8 +24,10 @@ public:
   double progress() const;
   void setProgress(double progress);
 
-  uint64_t id() const;
-  void setId(const uint64_t &id);
+  int64_t id() const;
+  void setId(const int64_t &id);
+
+  QList<QGeoCoordinate> tile() const;
 
 signals:
   void progressChanged();
@@ -35,5 +36,5 @@ signals:
 private:
   void init();
   double _progress;
-  long _id;
+  int64_t _id;
 };

src/MeasurementComplexItem/nemo_interface/QNemoProgress.cc

-#include "QNemoProgress.h"
-

src/MeasurementComplexItem/nemo_interface/QNemoProgress.h

-#pragma once
-
-#include <QVector>
-
-#include "ros_bridge/include/messages/nemo_msgs/progress.h"
-
-namespace nemo = ros_bridge::messages::nemo_msgs;
-typedef nemo::progress::GenericProgress<int, QVector> QNemoProgress;

src/MeasurementComplexItem/nemo_interface/SnakeTileLocal.h

-#pragma once
-#include "geometry/GenericPolygon.h"
-
-using SnakeTileLocal = GenericPolygon<QPointF, std::vector>;

src/MeasurementComplexItem/nemo_interface/SnakeTiles.h

-#pragma once
-#include "MeasurementTile.h"
-#include "Wima/Geometry/GenericPolygonArray.h"
-
-using SnakeTiles =  GenericPolygonArray<MeasurementTile, QVector>;

src/MeasurementComplexItem/nemo_interface/SnakeTilesLocal.h

-#pragma once
-
-#include "MeasurementComplexItem//geometry/GenericPolygonArray.h"
-#include "MeasurementComplexItem//nemo_interface/SnakeTileLocal.h"
-#include <vector>
-typedef GenericPolygonArray<SnakeTileLocal, std::vector> SnakeTilesLocal;

src/MeasurementComplexItem/nemo_interface/Task.cpp

+#include "Task.h"
+
+namespace nemo_interface {
+
+Task::Task(const Task::Functor &onExec)
+    : _onExec(onExec), _isExpired([] { return false; }),
+      _isReady([] { return true; }) {}
+
+void Task::exec(std::promise<QVariant> p) { this->_onExec(std::move(p)); }
+
+} // namespace nemo_interface

src/MeasurementComplexItem/nemo_interface/Task.h

+#ifndef COMMAND_H
+#define COMMAND_H
+
+#include <functional>
+#include <future>
+
+#include <QVariant>
+
+namespace nemo_interface {
+
+class Task {
+public:
+  typedef std::function<void(std::promise<QVariant> p)> Functor;
+  typedef std::function<bool()> BooleanFunctor;
+
+  Task(const Functor &onExec);
+
+  void exec(std::promise<QVariant> p);
+  bool isReady();   // default == true
+  bool isExpired(); // default == false
+
+  void setOnExec(const Functor &onExec);
+  void setIsReady(const BooleanFunctor &isReady);
+  void setIsExpired(const BooleanFunctor &isExpired);
+
+private:
+  Functor _onExec;
+  BooleanFunctor _isExpired;
+  BooleanFunctor _isReady;
+};
+} // namespace nemo_interface
+
+#endif // COMMAND_H

src/MeasurementComplexItem/nemo_interface/tileHelper.h

+#ifndef TILEHELPER_H
+#define TILEHELPER_H
+
+#include <iostream>
+
+namespace nemo_interface {
+
+namespace coordinate {
+template <class Coordinate> inline std::int64_t getHash(const Coordinate &t) {
+  return double(t.latitude()) ^ double(t.longitude()) ^ double(t.altitude());
+}
+} // namespace coordinate
+
+namespace coordinate_array {
+template <class Container> inline std::int64_t getHash(const Container &c) {
+  std::int64_t hash = 0;
+  for (const auto &entry : c) {
+    hash ^= coordinate::getHash(entry);
+  }
+}
+} // namespace coordinate_array
+
+namespace tile {
+template <class Tile> inline std::int64_t getHash(const Tile &t) {
+  return coordinate_array::getHash(t.tile());
+}
+} // namespace tile
+
+namespace tile_array {
+template <class Container> inline getHash(const Container &c) {
+  std::int64_t hash = 0;
+  for (const auto &entry : c) {
+    hash ^= tile::getHash(entry);
+  }
+}
+} // namespace tile_array
+
+namespace labeled_progress {
+template <class LabeledProgress> inline getHash(const LabeledProgress &lp) {
+  return std::int64_t(lp.id()) ^ double(lp.progress());
+}
+
+namespace progress_array {
+template <class Container> inline getHash(const Container &c) {
+  std::int64_t hash = 0;
+  for (const auto &entry : c) {
+    hash ^= labeled_progress::getHash(entry);
+  }
+}
+
+} // namespace progress_array
+
+} // namespace labeled_progress
+
+} // namespace nemo_interface
+
+#endif // TILEHELPER_H

src/comm/ros_bridge/include/RosBridgeClient.cpp

@@ -302,6 +302,8 @@ void RosbridgeWsClient::run() {
   });
 }
 
+bool RosbridgeWsClient::running() { return !this->stopped->load(); }
+
 void RosbridgeWsClient::stop() {
   if (stopped->load())
     return;

src/comm/ros_bridge/include/RosBridgeClient.h

 #pragma once
 
 /*
-*  Created on: Apr 16, 2018
-*      Author: Poom Pianpak
-*/
+ *  Created on: Apr 16, 2018
+ *      Author: Poom Pianpak
+ */
 
 #include "Simple-WebSocket-Server/client_ws.hpp"
 
 #include "rapidjson/include/rapidjson/document.h"
-#include "rapidjson/include/rapidjson/writer.h"
 #include "rapidjson/include/rapidjson/stringbuffer.h"
+#include "rapidjson/include/rapidjson/writer.h"
 
+#include <deque>
 #include <functional>
 #include <mutex>
-#include <tuple>
-#include <deque>
 #include <thread>
+#include <tuple>
 
 bool is_valid_port_path(std::string server_port_path);
 
 using WsClient = SimpleWeb::SocketClient<SimpleWeb::WS>;
-using InMessage =  std::function<void(std::shared_ptr<WsClient::Connection>, std::shared_ptr<WsClient::InMessage>)>;
+using InMessage = std::function<void(std::shared_ptr<WsClient::Connection>,
+                                     std::shared_ptr<WsClient::InMessage>)>;
 
 template <typename T>
-constexpr typename std::underlying_type<T>::type integral(T value)
-{
-    return static_cast<typename std::underlying_type<T>::type>(value);
+constexpr typename std::underlying_type<T>::type integral(T value) {
+  return static_cast<typename std::underlying_type<T>::type>(value);
 }
 
-class RosbridgeWsClient
-{
-    enum class EntryType{
-        SubscribedTopic,
-        AdvertisedTopic,
-        AdvertisedService,
-    };
-
-    using EntryData = std::tuple<EntryType,
-                             std::string /*service/topic name*/,
-                             std::string /*client name*/,
-                             std::weak_ptr<WsClient> /*client*/>;
-
-    enum class EntryEnum{
-        EntryType = 0,
-        ServiceTopicName = 1,
-        ClientName = 2,
-        WPClient = 3
-    };
+class RosbridgeWsClient {
+  enum class EntryType {
+    SubscribedTopic,
+    AdvertisedTopic,
+    AdvertisedService,
+  };
+
+  using EntryData = std::tuple<EntryType, std::string /*service/topic name*/,
+                               std::string /*client name*/,
+                               std::weak_ptr<WsClient> /*client*/>;
+
+  enum class EntryEnum {
+    EntryType = 0,
+    ServiceTopicName = 1,
+    ClientName = 2,
+    WPClient = 3
+  };
 
   const std::string server_port_path;
   std::unordered_map<std::string /*client name*/,
-                     std::shared_ptr<WsClient> /*client*/> client_map;
+                     std::shared_ptr<WsClient> /*client*/>
+      client_map;
   std::deque<EntryData> service_topic_list;
   std::mutex mutex;
   std::shared_ptr<std::atomic_bool> isConnected;
@@ -59,38 +58,39 @@ class RosbridgeWsClient
   std::string available_services;
   std::shared_ptr<std::thread> periodic_thread;
 
-
-
-  void start(const std::string& client_name, std::shared_ptr<WsClient> client, const std::string& message);
+  void start(const std::string &client_name, std::shared_ptr<WsClient> client,
+             const std::string &message);
 
 public:
-  RosbridgeWsClient(const std::string& server_port_path);
-  RosbridgeWsClient(const std::string& server_port_path, bool run);
+  RosbridgeWsClient(const std::string &server_port_path);
+  RosbridgeWsClient(const std::string &server_port_path, bool run);
 
   ~RosbridgeWsClient();
 
- bool connected();
+  bool connected();
 
- void run();
- void stop();
- void reset();
+  void run();
+  bool running();
+  void stop();
+  void reset();
 
   // Adds a client to the client_map.
-  void addClient(const std::string& client_name);
+  void addClient(const std::string &client_name);
 
-  std::shared_ptr<WsClient> getClient(const std::string& client_name);
+  std::shared_ptr<WsClient> getClient(const std::string &client_name);
 
-  void stopClient(const std::string& client_name);
+  void stopClient(const std::string &client_name);
 
-  void removeClient(const std::string& client_name);
+  void removeClient(const std::string &client_name);
 
   //!
   //! \brief Returns a string containing all advertised topics.
   //! \return Returns a string containing all advertised topics.
   //!
-  //! \note This function will wait until the /rosapi/topics service is available.
-  //! \note Call connected() in advance to ensure that a connection was established.
-  //! \pre Connection must be established, \see \fn connected().
+  //! \note This function will wait until the /rosapi/topics service is
+  //! available. \note Call connected() in advance to ensure that a connection
+  //! was established. \pre Connection must be established, \see \fn
+  //! connected().
   //!
   std::string getAdvertisedTopics();
 
@@ -98,16 +98,18 @@ public:
   //! \brief Returns a string containing all advertised services.
   //! \return Returns a string containing all advertised services.
   //!
-  //! \note This function will wait until the /rosapi/services service is available.
-  //! \note Call connected() in advance to ensure that a connection was established.
+  //! \note This function will wait until the /rosapi/services service is
+  //! available. \note Call connected() in advance to ensure that a connection
+  //! was established.
   //!
   std::string getAdvertisedServices();
 
   bool topicAvailable(const std::string &topic);
 
-  // Gets the client from client_map and starts it. Advertising is essentially sending a message.
-  // One client per topic!
-  void advertise(const std::string& client_name, const std::string& topic, const std::string& type, const std::string& id = "");
+  // Gets the client from client_map and starts it. Advertising is essentially
+  // sending a message. One client per topic!
+  void advertise(const std::string &client_name, const std::string &topic,
+                 const std::string &type, const std::string &id = "");
 
   //!
   //! \brief Unadvertises the topice \p topic
@@ -115,19 +117,19 @@ public:
   //! \param id
   //! \pre The topic \p topic must be advertised, \see topicAvailable().
   //!
-  void unadvertise(const std::string& topic,
-                   const std::string& id = "");
+  void unadvertise(const std::string &topic, const std::string &id = "");
 
   void unadvertiseAll();
 
-    //!
-    //! \brief Publishes the message \p msg to the topic \p topic.
-    //! \param topic The topic to publish the message.
-    //! \param msg The message to publish.
-    //! \param id
-    //! \pre The topic \p topic must be advertised, \see topicAvailable().
-    //!
-  void publish(const std::string& topic, const rapidjson::Document& msg, const std::string& id = "");
+  //!
+  //! \brief Publishes the message \p msg to the topic \p topic.
+  //! \param topic The topic to publish the message.
+  //! \param msg The message to publish.
+  //! \param id
+  //! \pre The topic \p topic must be advertised, \see topicAvailable().
+  //!
+  void publish(const std::string &topic, const rapidjson::Document &msg,
+               const std::string &id = "");
 
   //!
   //! \brief Subscribes the client \p client_name to the topic \p topic.
@@ -142,57 +144,71 @@ public:
   //! \param compression
   //! \pre The topic \p topic must be advertised, \see topicAvailable().
   //!
-  void subscribe(const std::string& client_name, const std::string& topic, const InMessage& callback, const std::string& id = "", const std::string& type = "", int throttle_rate = -1, int queue_length = -1, int fragment_size = -1, const std::string& compression = "");
+  void subscribe(const std::string &client_name, const std::string &topic,
+                 const InMessage &callback, const std::string &id = "",
+                 const std::string &type = "", int throttle_rate = -1,
+                 int queue_length = -1, int fragment_size = -1,
+                 const std::string &compression = "");
 
-  void unsubscribe(const std::string& topic,
-                   const std::string& id = "");
+  void unsubscribe(const std::string &topic, const std::string &id = "");
 
   void unsubscribeAll();
 
-  void advertiseService(const std::string& client_name, const std::string& service, const std::string& type, const InMessage& callback);
+  void advertiseService(const std::string &client_name,
+                        const std::string &service, const std::string &type,
+                        const InMessage &callback);
 
-  void unadvertiseService(const std::string& service);
+  void unadvertiseService(const std::string &service);
 
   void unadvertiseAllServices();
 
-  void serviceResponse(const std::string& service, const std::string& id, bool result, const rapidjson::Document& values);
+  void serviceResponse(const std::string &service, const std::string &id,
+                       bool result, const rapidjson::Document &values);
 
-  void callService(const std::string& service, const InMessage& callback, const rapidjson::Document& args = {}, const std::string& id = "", int fragment_size = -1, const std::string& compression = "");
+  void callService(const std::string &service, const InMessage &callback,
+                   const rapidjson::Document &args = {},
+                   const std::string &id = "", int fragment_size = -1,
+                   const std::string &compression = "");
 
   //!
   //! \brief Checks if the service \p service is available.
   //! \param service Service name.
   //! \return Returns true if the service is available, false either.
-  //! \note Don't call this function to frequently. Use \fn waitForService() instead.
+  //! \note Don't call this function to frequently. Use \fn waitForService()
+  //! instead.
   //!
-  bool serviceAvailable(const std::string& service);
+  bool serviceAvailable(const std::string &service);
 
   //!
   //! \brief Waits until the service with the name \p service is available.
   //! \param service Service name.
   //! @note This function will block as long as the service is not available.
   //!
-  void waitForService(const std::string& service);
+  void waitForService(const std::string &service);
 
   //!
   //! \brief Waits until the service with the name \p service is available.
   //! \param service Service name.
   //! \param stop Flag to stop waiting.
-  //! @note This function will block as long as the service is not available or \p stop is false.
+  //! @note This function will block as long as the service is not available or
+  //! \p stop is false.
   //!
-  void waitForService(const std::string& service, const std::function<bool(void)> stop);
+  void waitForService(const std::string &service,
+                      const std::function<bool(void)> stop);
 
   //!
   //! \brief Waits until the topic with the name \p topic is available.
   //! \param topic Topic name.
   //! @note This function will block as long as the topic is not available.
   //!
-  void waitForTopic(const std::string& topic);
+  void waitForTopic(const std::string &topic);
   //!
   //! \brief Waits until the topic with the name \p topic is available.
   //! \param topic Topic name.
   //! \param stop Flag to stop waiting.
-  //! @note This function will block as long as the topic is not available or \p stop is false.
+  //! @note This function will block as long as the topic is not available or \p
+  //! stop is false.
   //!
-  void waitForTopic(const std::string& topic, const std::function<bool(void)> stop);
+  void waitForTopic(const std::string &topic,
+                    const std::function<bool(void)> stop);
 };

src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.cpp

-#include "polygon_array.h"
-
-std::string ros_bridge::messages::jsk_recognition_msgs::polygon_array::messageType(){
-    return "jsk_recognition_msgs/PolygonArray";
-}
-

src/comm/ros_bridge/include/messages/jsk_recognition_msgs/polygon_array.h

-#pragma once
-#include "ros_bridge/rapidjson/include/rapidjson/document.h"
-#include "ros_bridge/include/message_traits.h"
-#include "ros_bridge/include/messages/geometry_msgs/polygon_stamped.h"
-#include "ros_bridge/include/messages/std_msgs/header.h"
-
-#include <type_traits>
-#include <vector>
-
-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 jsk_recognition_msgs {
-//! @brief Namespace containing methodes for jsk_recognition_msgs/PolygonArray message generation.
-namespace polygon_array {
-
-
-std::string messageType();
-
-
-//! @brief C++ representation of jsk_recognition_msgs/PolygonArray
-template <class PolygonType = geometry_msgs::polygon_stamped::PolygonStamped,
-          template <class, class...> class ContainerType = std::vector,
-          class HeaderType =  std_msgs::header::Header,
-          class IntType = long,
-          class FloatType = double>
-class GenericPolygonArray{
-public:
-    GenericPolygonArray() {}
-    GenericPolygonArray(const GenericPolygonArray &other)
-      : _header(other.header())
-      , _polygons(other.polygons())
-      , _labels(other.labels())
-      , _likelihood(other.likelihood())
-    {}
-    GenericPolygonArray(const HeaderType                   &header,
-                 const ContainerType<PolygonType>   &polygons,
-                 const ContainerType<IntType>      &labels,
-                 const ContainerType<FloatType>      &likelihood)
-      : _header(header)
-      , _polygons(polygons)
-      , _labels(labels)
-      , _likelihood(likelihood)
-    {}
-
-    const HeaderType                 &header()     const {return _header;}
-          HeaderType                 &header()           {return _header;}
-    const ContainerType<PolygonType> &polygons()   const {return _polygons;}
-          ContainerType<PolygonType> &polygons()         {return _polygons;}
-    const ContainerType<IntType>     &labels()     const {return _labels;}
-          ContainerType<IntType>     &labels()           {return _labels;}
-    const ContainerType<FloatType>   &likelyhood() const {return _likelihood;}
-          ContainerType<FloatType>   &likelyhood()       {return _likelihood;}
-
-
-private:
-    HeaderType                  _header;
-    ContainerType<PolygonType>  _polygons;
-    ContainerType<IntType>      _labels;
-    ContainerType<FloatType>    _likelihood;
-};
-typedef GenericPolygonArray<> PolygonArray;
-
-
-namespace detail {
-    //! Helper functions to generate Json entries for labels and likelihood.
-
-    //! \note \p p has member \fn labels().
-    template <class PolygonArrayType, int  k>
-    void labelsToJson(const PolygonArrayType &p,
-                      rapidjson::Value &labels,
-                      rapidjson::Document::AllocatorType &allocator,
-                      traits::Int2Type<k>){
-        for(unsigned long i=0; i < (unsigned long)p.labels().size(); ++i)
-            labels.PushBack(rapidjson::Value().SetUint(p.labels()[i]), allocator);
-    }
-
-    //! \note \p p has no member \fn labels().
-    template <class PolygonArrayType>
-    void labelsToJson(const PolygonArrayType &p,
-                      rapidjson::Value &labels,
-                      rapidjson::Document::AllocatorType &allocator,
-                      traits::Int2Type<0>){
-        for(unsigned long i=0; i < (unsigned long)(p.polygons().size()); ++i)
-            labels.PushBack(rapidjson::Value().SetUint(0), allocator); // use zero!
-    }
-
-    //! \note \p p has member \fn likelihood().
-    template <class PolygonArrayType, int  k>
-    void likelihoodToJson(const PolygonArrayType &p,
-                          rapidjson::Value &likelyhood,
-                          rapidjson::Document::AllocatorType &allocator,
-                          traits::Int2Type<k>){
-        p.likelyhood().clear();
-        for(unsigned long i=0; i < (unsigned long)p.likelyhood().size(); ++i)
-            likelyhood.PushBack(rapidjson::Value().SetFloat(p.likelyhood()[i]), allocator);
-    }
-
-    //! \note \p p has no member \fn likelihood().
-    template <class PolygonArrayType>
-    void likelihoodToJson(const PolygonArrayType &p,
-                          rapidjson::Value &likelyhood,
-                          rapidjson::Document::AllocatorType &allocator,
-                          traits::Int2Type<0>){
-        for(unsigned long i=0; i < (unsigned long)p.polygons().size(); ++i)
-            likelyhood.PushBack(rapidjson::Value().SetFloat(0), allocator); // use zero!
-    }
-
-    //! \note \p p has member \fn labels().
-    template <class PolygonArrayType, int k>
-    void setLabels(const rapidjson::Value &doc,
-                   PolygonArrayType &p,
-                   traits::Int2Type<k>){
-        p.labels().clear();
-        for(unsigned long i=0; i < (unsigned long)doc.Size(); ++i)
-            p.labels().push_back(doc[i]);
-    }
-
-    //! \note \p p has no member \fn labels().
-    template <class PolygonArrayType>
-    void setLabels(const rapidjson::Value &doc,
-                   PolygonArrayType &p,
-                   traits::Int2Type<0>){
-        (void)doc;
-        (void)p;
-    }
-
-    //! \note \p p has member \fn likelihood().
-    template <class PolygonArrayType, int k>
-    void setLikelihood(const rapidjson::Value &doc,
-                       PolygonArrayType &p,
-                       traits::Int2Type<k>){
-        for(unsigned long i=0; i < (unsigned long)doc.Size(); ++i)
-            p.likelihood().push_back(doc[i]);
-    }
-
-    //! \note \p p has no member \fn likelihood().
-    template <class PolygonArrayType>
-    void setLikelihood(const rapidjson::Value &doc,
-                       PolygonArrayType &p,
-                       traits::Int2Type<0>){
-        (void)doc;
-        (void)p;
-    }
-
-    template <class PolygonArrayType, class HeaderType>
-    bool _toJson(const PolygonArrayType &p,
-                 const HeaderType &h,
-                 rapidjson::Value &value,
-                 rapidjson::Document::AllocatorType &allocator)
-    {
-        using namespace std_msgs;
-        using namespace geometry_msgs;
-
-        rapidjson::Document jHeader(rapidjson::kObjectType);
-        if (!header::toJson(h, jHeader, allocator)){
-            assert(false);
-            return false;
-        }
-        value.AddMember("header", jHeader, allocator);
-
-        rapidjson::Value jPolygons(rapidjson::kArrayType);
-        for(unsigned long i=0; i < (unsigned long)(p.polygons().size()); ++i){
-            rapidjson::Document jPolygon(rapidjson::kObjectType);
-            if (!polygon_stamped::toJson(p.polygons()[i].polygon(), h, jPolygon, allocator)){
-                assert(false);
-                return false;
-            }
-            jPolygons.PushBack(jPolygon, allocator);
-        }
-        value.AddMember("polygons", jPolygons, allocator);
-
-
-        rapidjson::Value jLabels(rapidjson::kArrayType);
-        typedef traits::HasMemberLabels<PolygonArrayType> HasLabels;
-        detail::labelsToJson(p, jLabels, allocator, traits::Int2Type<HasLabels::value>());
-        value.AddMember("labels", jLabels, allocator);
-
-
-        rapidjson::Value jLikelihood(rapidjson::kArrayType);
-        typedef traits::HasMemberLikelihood<PolygonArrayType> HasLikelihood;
-        detail::likelihoodToJson(p, jLikelihood, allocator, traits::Int2Type<HasLikelihood::value>());
-        value.AddMember("likelihood", jLikelihood, allocator);
-
-        return true;
-    }
-
-    template<class PolygonArrayType, int k>
-    bool _toJson(const PolygonArrayType &p,
-                 rapidjson::Value &value,
-                 rapidjson::Document::AllocatorType &allocator,
-                 traits::Int2Type<k>)
-    {
-        // U has member header(), use integraded header.
-        return _toJson(p, p.header(), value, allocator);
-    }
-
-    template<class PolygonArrayType>
-    bool _toJson(const PolygonArrayType &p,
-                 rapidjson::Value &value,
-                 rapidjson::Document::AllocatorType &allocator,
-                 traits::Int2Type<0>)
-    {
-        // U has no member header(), generate one on the fly.
-        using namespace std_msgs::header;
-        return _toJson(p, Header(), value, allocator);
-    }
-}
-
-
-//!
-//! Create PolygonArray message from \p p and \p h. \p p doesn't have it's own header.
-//! \param p Class implementing the PolygonArray interface.
-//! \param h Class implementing the Header interface.
-//! \param doc Rapidjson document used to store the PolygonArray message.
-//! \param allocator Allocator used by doc. Can be obtained e.g. by calling doc.getAllocator().
-//!
-//! \note The \fn labels() and \fn likelihood() members are optinal. If any of them is missing they
-//! will be replaced by arrays filled with zero and size p.polygons.size().
-//!
-//! \note If this function is called, the headers in p.polygons[i] (entries implement the the PolygonStamped interface)
-//! are ignored.
-template <class PolygonArrayType, class HeaderType>
-bool toJson(const PolygonArrayType &p,
-            const HeaderType &h,
-            rapidjson::Value &value,
-            rapidjson::Document::AllocatorType &allocator)
-{
-    return detail::_toJson(p, h, value, allocator);
-}
-
-
-
-//!
-//! Create PolygonArray message from \p p. \p p contains a header.
-//! \param p Class implementing the PolygonArrayType interface.
-//! \param doc Rapidjson document used to store the PolygonArray message.
-//! \param allocator Allocator used by doc. Can be obtained e.g. by calling doc.getAllocator().
-//!
-//! \note The \fn labels() and \fn likelihood() members are optinal. If any of them is missing they
-//! will be replaced by arrays filled with zero and size p.polygons.size().
-//!
-//! \note If the header() function is missing, a default constructed header is used.
-template <class PolygonArrayType>
-bool toJson(const PolygonArrayType &p,
-            rapidjson::Value &value,
-            rapidjson::Document::AllocatorType &allocator)
-{
-    typedef traits::HasMemberHeader<PolygonArrayType> HasHeader;
-    return detail::_toJson(p, value, allocator, traits::Int2Type<HasHeader::value>());
-}
-
-
-
-template <class PolygonArrayType>
-bool fromJson(const rapidjson::Value &value,
-              PolygonArrayType &p)
-{
-    using namespace geometry_msgs;
-    if ( !value.HasMember("header")){
-        assert(false);
-        return false;
-    }
-    if ( !value.HasMember("polygons") || !value["polygons"].IsArray() ){
-        assert(false);
-        return false;
-    }
-    if ( !value.HasMember("labels") || !value["labels"].IsArray() ){
-        assert(false);
-        return false;
-    }
-    if ( !value.HasMember("likelihood") || !value["likelihood"].IsArray() ){
-        assert(false);
-        return false;
-    }
-
-
-    typedef traits::HasMemberHeader<PolygonArrayType> HasHeader;
-    if ( !polygon_stamped::detail::setHeader(value["header"],
-                                             p,
-                                             traits::Int2Type<HasHeader::value>())){
-        assert(false);
-        return false;
-    }
-
-    const auto &jPolygonStamped = value["polygons"];
-    p.polygons().clear();
-    p.polygons().reserve(jPolygonStamped.Size());
-    typedef decltype (p.polygons()[0]) PolyStampedCVR;
-    typedef typename std::remove_cv_t<
-            typename std::remove_reference_t<PolyStampedCVR>>
-            PolyStamped;
-    for (unsigned int i=0; i < jPolygonStamped.Size(); ++i) {
-        if ( !jPolygonStamped[i].HasMember("header") ){
-            assert(false);
-            return false;
-        }
-        PolyStamped polygonStamped;
-
-        if ( !polygon_stamped::detail::setHeader(jPolygonStamped[i]["header"],
-                                                 polygonStamped,
-                                                 traits::Int2Type<HasHeader::value>())){
-            assert(false);
-            return false;
-        }
-
-        if ( !polygon::fromJson(jPolygonStamped[i]["polygon"], polygonStamped.polygon())){
-            assert(false);
-            return false;
-        }
-        p.polygons().push_back(std::move(polygonStamped));
-    }
-
-    typedef traits::HasMemberLabels<PolygonArrayType> HasLabels;
-    detail::setLabels(value["labels"], p, traits::Int2Type<HasLabels::value>());
-
-    typedef traits::HasMemberLikelihood<PolygonArrayType> HasLikelihood;
-    detail::setLikelihood(value["likelihood"], p, traits::Int2Type<HasLikelihood::value>());
-
-    return true;
-}
-
-
-} // namespace polygon_array
-} // namespace geometry_msgs
-} // namespace messages
-} // namespace ros_bridge

src/comm/ros_bridge/include/messages/nemo_msgs/heartbeat.h

@@ -5,46 +5,48 @@
 namespace ros_bridge {
 //! @brief Namespace containing classes and methodes ros message generation.
 namespace messages {
-//! @brief Namespace containing classes and methodes for geometry_msgs generation.
+//! @brief Namespace containing classes and methodes for geometry_msgs
+//! generation.
 namespace nemo_msgs {
-//! @brief Namespace containing methodes for geometry_msgs/GeoPoint message generation.
+//! @brief Namespace containing methodes for geometry_msgs/GeoPoint message
+//! generation.
 namespace heartbeat {
 
-
 std::string messageType();
 
-
 //! @brief C++ representation of nemo_msgs/Heartbeat message
-class Heartbeat{
+class Heartbeat {
 public:
-    Heartbeat() : _status(0){}
-    Heartbeat(int status) :_status(status){}
-    Heartbeat(const Heartbeat &heartbeat) : _status(heartbeat.status()){}
+  Heartbeat() : _status(0) {}
+  Heartbeat(int status) : _status(status) {}
+  Heartbeat(const Heartbeat &heartbeat) : _status(heartbeat.status()) {}
+
+  bool operator==(const Heartbeat &hb) { return hb._status == this->_status; }
+  bool operator!=(const Heartbeat &hb) { return !operator==(hb); }
+
+  virtual int status(void) const { return _status; }
+  virtual void setStatus(int status) { _status = status; }
 
-    virtual int     status      (void)const    {return _status;}
-    virtual void    setStatus   (int status)    {_status = status;}
 protected:
-    int _status;
+  int _status;
 };
 
-
 template <class HeartbeatType>
-bool toJson(const HeartbeatType &heartbeat, rapidjson::Value &value, rapidjson::Document::AllocatorType &allocator)
-{
-    value.AddMember("status", std::int32_t(heartbeat.status()), allocator);
-    return true;
+bool toJson(const HeartbeatType &heartbeat, rapidjson::Value &value,
+            rapidjson::Document::AllocatorType &allocator) {
+  value.AddMember("status", std::int32_t(heartbeat.status()), allocator);
+  return true;
 }
 
 template <class HeartbeatType>
-bool fromJson(const rapidjson::Value &value, HeartbeatType &heartbeat)
-{
-    if (!value.HasMember("status") || !value["status"].IsInt()){
-        assert(false);
-        return false;
-    }
-
-    heartbeat.setStatus(value["status"].GetInt());
-    return true;
+bool fromJson(const rapidjson::Value &value, HeartbeatType &heartbeat) {
+  if (!value.HasMember("status") || !value["status"].IsInt()) {
+    assert(false);
+    return false;
+  }
+
+  heartbeat.setStatus(value["status"].GetInt());
+  return true;
 }
 
 } // namespace heartbeat

src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.h

@@ -41,8 +41,8 @@ protected:
 template <class LabeledProgressType>
 bool toJson(const LabeledProgressType &lp, rapidjson::Value &value,
             rapidjson::Document::AllocatorType &allocator) {
-  value.AddMember(idKey, lb.id(), allocator);
-  value.AddMember(progressKey, lb.progress(), allocator);
+  value.AddMember(idKey, lp.id(), allocator);
+  value.AddMember(progressKey, lp.progress(), allocator);
   return true;
 }
 

src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp

 #include "tile.h"
 
-tile::tile()
-{
-
+std::string ros_bridge::messages::nemo_msgs::tile::messageType() {
+  return "nemo_msgs/Tile";
 }

src/comm/ros_bridge/include/messages/nemo_msgs/tile.h

@@ -13,7 +13,7 @@ namespace messages {
 namespace nemo_msgs {
 //! @brief Namespace containing methodes for nemo_msgs/tile message
 //! generation.
-namespace labeled_progress {
+namespace tile {
 
 std::string messageType();
 
@@ -24,7 +24,8 @@ const char *tileKey = "tile";
 } // namespace
 
 //! @brief C++ representation of nemo_msgs/tile message
-template <class GeoPoint, template <class, class> class Container = std::vector>
+template <class GeoPoint = geographic_msgs::geo_point::GeoPoint,
+          template <class, class...> class Container = std::vector>
 class GenericTile {
 public:
   typedef Container<GeoPoint> GeoContainer;
@@ -52,25 +53,30 @@ typedef GenericTile<> Tile;
 template <class TileType>
 bool toJson(const TileType &tile, rapidjson::Value &value,
             rapidjson::Document::AllocatorType &allocator) {
-  value.AddMember(idKey, tile.id(), allocator);
-  value.AddMember(progressKey, tile.progress(), allocator);
+
+  using namespace rapidjson;
+  value.AddMember(Value(idKey, allocator), Value(tile.id()), allocator);
+  value.AddMember(Value(progressKey, allocator), Value(tile.progress()),
+                  allocator);
 
   using namespace messages::geographic_msgs;
   rapidjson::Value geoPoints(rapidjson::kArrayType);
   for (unsigned long i = 0; i < std::uint64_t(tile.tile().size()); ++i) {
-    rapidjson::Document geoPoint(rapidjson::kObjectType);
+    rapidjson::Value geoPoint(rapidjson::kObjectType);
     if (!geo_point::toJson(tile.tile()[i], geoPoint, allocator)) {
       return false;
     }
     geoPoints.PushBack(geoPoint, allocator);
   }
-  value.AddMember(tileKey, geoPoints, allocator);
+
+  rapidjson::Value key(tileKey, allocator);
+  value.AddMember(key, geoPoints, allocator);
 
   return true;
 }
 
-template <class ProgressType>
-bool fromJson(const rapidjson::Value &value, ProgressType &p) {
+template <class TileType>
+bool fromJson(const rapidjson::Value &value, TileType &p) {
   if (!value.HasMember(progressKey) || !value[progressKey].IsDouble()) {
     return false;
   }
@@ -79,13 +85,34 @@ bool fromJson(const rapidjson::Value &value, ProgressType &p) {
     return false;
   }
 
+  if (!value.HasMember(tileKey) || !value[tileKey].IsArray()) {
+    return false;
+  }
+
   p.setId(value[idKey].GetInt());
   p.setProgress(value[progressKey].GetDouble());
 
+  using namespace geographic_msgs;
+
+  const auto &jsonArray = value[tileKey].GetArray();
+  p.tile().clear();
+  p.tile().reserve(jsonArray.Size());
+  typedef decltype(p.tile()[0]) PointTypeCVR;
+  typedef
+      typename std::remove_cv_t<typename std::remove_reference_t<PointTypeCVR>>
+          PointType;
+  for (long i = 0; i < jsonArray.Size(); ++i) {
+    PointType pt;
+    if (!geo_point::fromJson(jsonArray[i], pt)) {
+      return false;
+    }
+    p.tile().push_back(std::move(pt));
+  }
+
   return true;
 }
 
-} // namespace labeled_progress
+} // namespace tile
 } // namespace nemo_msgs
 } // namespace messages
 } // namespace ros_bridge