temp

qgroundcontrol.pro

@@ -453,6 +453,8 @@ HEADERS += \
     src/MeasurementComplexItem/geometry/TileDiff.h \
     src/MeasurementComplexItem/geometry/geometry.h \
     src/MeasurementComplexItem/HashFunctions.h \
+    src/MeasurementComplexItem/nemo_interface/Command.h \
+    src/MeasurementComplexItem/nemo_interface/CommandDispatcher.h \
     src/MeasurementComplexItem/nemo_interface/MeasurementTile.h \
     src/QmlControls/QmlUnitsConversion.h \
     src/MeasurementComplexItem/geometry/GeoArea.h \
@@ -534,6 +536,8 @@ 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/Vehicle/VehicleEscStatusFactGroup.cc \
     src/MeasurementComplexItem/AreaData.cc \

src/MeasurementComplexItem/NemoInterface.cpp

@@ -40,47 +40,69 @@ using SharedLock = std::shared_lock<std::shared_timed_mutex>;
 using JsonDocUPtr = ros_bridge::com_private::JsonDocUPtr;
 
 class NemoInterface::Impl {
-  using TimePoint = std::chrono::time_point<std::chrono::high_resolution_clock>;
-
 public:
+  enum class STATE {
+    STOPPED,
+    RUNNING,
+    WEBSOCKET_DETECTED,
+    HEARTBEAT_DETECTED,
+    TRY_TOPIC_SERVICE_SETUP,
+    READY,
+    SYNCHRONIZING,
+    TIMEOUT,
+    INVALID_HEARTBEAT
+  }
+
   Impl(NemoInterface *p);
 
   void start();
   void stop();
 
+  // Tile editing.
+  // 	Functions that require communication to device.
   void addTiles(const TilePtrArray &tileArray);
   void addTiles(const TileArray &tileArray);
   void removeTiles(const IDArray &idArray);
   void clearTiles();
+
+  // 	Functions that don't require communication to device.
   TileArray getTiles(const IDArray &idArray);
   TileArray getAllTiles();
   LogicalArray containsTiles(const IDArray &idArray);
-  TileArray extractTiles(const IDArray &idArray);
   std::size_t size();
   bool empty();
-  void publishTileData();
+
+  // Progress.
+  ProgressArray getProgress();
+  ProgressArray getProgress(const IDArray &idArray);
 
   NemoInterface::STATUS status();
   bool running();
 
 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;
+
+  void _addTiles(const TileArray &tileArray);
+  void _removeTiles(const IDArray &idArray);
+  void _clearTiles();
+
   void doTopicServiceSetup();
   void loop();
-  static STATUS heartbeatToStatus(
-      const ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat &hb);
 
-  bool setStatus(NemoInterface::STATUS s);
+  bool _setState(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;
-  std::atomic<NemoInterface::STATUS> status_;
-
-  // Internals
-  std::atomic_bool running_;
-  std::atomic_bool topicServiceSetupDone;
-  ROSBridgePtr pRosBridge;
-  QTimer loopTimer;
-  NemoInterface *parent;
+
+  STATE _state;
+  ROSBridgePtr _pRosBridge;
+  TileMap _tileMap;
+  NemoInterface *_parent;
 };
 
 using StatusMap = std::map<NemoInterface::STATUS, QString>;
@@ -98,7 +120,7 @@ static StatusMap statusMap{
 
 NemoInterface::Impl::Impl(NemoInterface *p)
     : nextTimeout(TimePoint::max()), status_(STATUS::NOT_CONNECTED),
-      running_(false), topicServiceSetupDone(false), parent(p) {
+      running_(false), topicServiceSetupDone(false), _parent(p) {
 
   // ROS Bridge.
   WimaSettings *wimaSettings =
@@ -114,7 +136,7 @@ NemoInterface::Impl::Impl(NemoInterface *p)
           "Websocket connection string possibly invalid: " + connectionString +
               ". Trying to connect anyways.");
     }
-    this->pRosBridge.reset(
+    this->_pRosBridge.reset(
         new ros_bridge::ROSBridge(connectionString.toLocal8Bit().data()));
   };
   connect(connectionStringFact, &SettingsFact::rawValueChanged,
@@ -128,12 +150,73 @@ NemoInterface::Impl::Impl(NemoInterface *p)
 
 void NemoInterface::Impl::start() {
   this->running_ = true;
-  emit this->parent->runningChanged();
+  emit this->_parent->runningChanged();
 }
 
 void NemoInterface::Impl::stop() {
   this->running_ = false;
-  emit this->parent->runningChanged();
+  emit this->_parent->runningChanged();
+}
+
+TileArray NemoInterface::Impl::getTiles(const IDArray &idArray) {
+  TileArray tileArray;
+
+  for (const auto &id : idArray) {
+    const auto it = _tileMap.find(id);
+    if (it != _tileMap.end()) {
+      tileArray.append(it->second);
+    }
+  }
+
+  return tileArray;
+}
+
+TileArray NemoInterface::Impl::getAllTiles() {
+  TileArray tileArray;
+
+  for (const auto &entry : _tileMap) {
+    tileArray.append(entry.second);
+  }
+
+  return tileArray;
+}
+
+LogicalArray NemoInterface::Impl::containsTiles(const IDArray &idArray) {
+  LogicalArray logicalArray;
+
+  for (const auto &id : idArray) {
+    const auto &it = _tileMap.find(id);
+    logicalArray.append(it != _tileMap.end());
+  }
+
+  return logicalArray;
+}
+
+std::size_t NemoInterface::Impl::size() { return _tileMap.size(); }
+
+bool NemoInterface::Impl::empty() { return _tileMap.empty(); }
+
+ProgressArray NemoInterface::Impl::getProgress() {
+  ProgressArray progressArray;
+
+  for (const auto &entry : _tileMap) {
+    progressArray.append(TaggedProgress{entry.first, entry.second.progress()});
+  }
+
+  return progressArray;
+}
+
+ProgressArray NemoInterface::Impl::getProgress(const IDArray &idArray) {
+  ProgressArray progressArray;
+
+  for (const auto &id : idArray) {
+    const auto it = _tileMap.find(id);
+    if (id != _tileMap.end()) {
+      progressArray.append(TaggedProgress{it->first, it->second.progress()});
+    }
+  }
+
+  return progressArray;
 }
 
 void NemoInterface::Impl::setTileData(const TileData &tileData) {
@@ -198,14 +281,16 @@ void NemoInterface::Impl::publishTileData() {
   }
 }
 
-NemoInterface::STATUS NemoInterface::Impl::status() { return status_.load(); }
+NemoInterface::STATUS NemoInterface::Impl::status() {
+  return _translate(this->_state);
+}
 
 QVector<int> NemoInterface::Impl::progress() {
   SharedLock lk(this->progressMutex);
   return this->qProgress.progress();
 }
 
-bool NemoInterface::Impl::running() { return this->running_.load(); }
+bool NemoInterface::Impl::running() { return _running(this->_state); }
 
 void NemoInterface::Impl::doTopicServiceSetup() {
   using namespace ros_bridge::messages;
@@ -213,7 +298,7 @@ void NemoInterface::Impl::doTopicServiceSetup() {
   // snake tiles.
   {
     SharedLock lk(this->tilesENUMutex);
-    this->pRosBridge->advertiseTopic(
+    this->_pRosBridge->advertiseTopic(
         "/snake/tiles",
         jsk_recognition_msgs::polygon_array::messageType().c_str());
   }
@@ -221,12 +306,12 @@ void NemoInterface::Impl::doTopicServiceSetup() {
   // snake origin.
   {
     SharedLock lk(this->ENUOriginMutex);
-    this->pRosBridge->advertiseTopic(
+    this->_pRosBridge->advertiseTopic(
         "/snake/origin", geographic_msgs::geo_point::messageType().c_str());
   }
 
   // Subscribe nemo progress.
-  this->pRosBridge->subscribe(
+  this->_pRosBridge->subscribe(
       "/nemo/progress",
       /* callback */ [this](JsonDocUPtr pDoc) {
         std::lock(this->progressMutex, this->tilesENUMutex,
@@ -244,7 +329,7 @@ void NemoInterface::Impl::doTopicServiceSetup() {
           qgcApp()->informationMessageBoxOnMainThread(
               "Nemo Interface", "Invalid progress message received.");
         }
-        emit this->parent->progressChanged();
+        emit this->_parent->progressChanged();
 
         lk1.unlock();
         lk2.unlock();
@@ -252,15 +337,15 @@ void NemoInterface::Impl::doTopicServiceSetup() {
       });
 
   // Subscribe /nemo/heartbeat.
-  this->pRosBridge->subscribe(
+  this->_pRosBridge->subscribe(
       "/nemo/heartbeat",
       /* callback */ [this](JsonDocUPtr pDoc) {
         //        auto start = std::chrono::high_resolution_clock::now();
         nemo_msgs::heartbeat::Heartbeat heartbeatMsg;
         if (!nemo_msgs::heartbeat::fromJson(*pDoc, heartbeatMsg)) {
-          this->setStatus(STATUS::INVALID_HEARTBEAT);
+          this->_setState(STATUS::INVALID_HEARTBEAT);
         } else {
-          this->setStatus(heartbeatToStatus(heartbeatMsg));
+          this->_setState(heartbeatToStatus(heartbeatMsg));
         }
         if (this->status_ == STATUS::INVALID_HEARTBEAT) {
           UniqueLock lk(this->timeoutMutex);
@@ -280,7 +365,7 @@ void NemoInterface::Impl::doTopicServiceSetup() {
       });
 
   // Advertise /snake/get_origin.
-  this->pRosBridge->advertiseService(
+  this->_pRosBridge->advertiseService(
       "/snake/get_origin", "snake_msgs/GetOrigin",
       [this](JsonDocUPtr) -> JsonDocUPtr {
         using namespace ros_bridge::messages;
@@ -305,7 +390,7 @@ void NemoInterface::Impl::doTopicServiceSetup() {
       });
 
   // Advertise /snake/get_tiles.
-  this->pRosBridge->advertiseService(
+  this->_pRosBridge->advertiseService(
       "/snake/get_tiles", "snake_msgs/GetTiles",
       [this](JsonDocUPtr) -> JsonDocUPtr {
         SharedLock lk(this->tilesENUMutex);
@@ -331,25 +416,25 @@ void NemoInterface::Impl::doTopicServiceSetup() {
 void NemoInterface::Impl::loop() {
   // Check ROS Bridge status and do setup if necessary.
   if (this->running_) {
-    if (!this->pRosBridge->isRunning()) {
-      this->pRosBridge->start();
+    if (!this->_pRosBridge->isRunning()) {
+      this->_pRosBridge->start();
       this->loop();
-    } else if (this->pRosBridge->isRunning() && this->pRosBridge->connected() &&
-               !this->topicServiceSetupDone) {
+    } else if (this->_pRosBridge->isRunning() &&
+               this->_pRosBridge->connected() && !this->topicServiceSetupDone) {
       this->doTopicServiceSetup();
       this->topicServiceSetupDone = true;
 
-      this->setStatus(STATUS::WEBSOCKET_DETECTED);
-    } else if (this->pRosBridge->isRunning() &&
-               !this->pRosBridge->connected() && this->topicServiceSetupDone) {
-      this->pRosBridge->reset();
-      this->pRosBridge->start();
+      this->_setState(STATUS::WEBSOCKET_DETECTED);
+    } else if (this->_pRosBridge->isRunning() &&
+               !this->_pRosBridge->connected() && this->topicServiceSetupDone) {
+      this->_pRosBridge->reset();
+      this->_pRosBridge->start();
       this->topicServiceSetupDone = false;
 
-      this->setStatus(STATUS::TIMEOUT);
+      this->_setState(STATUS::TIMEOUT);
     }
-  } else if (this->pRosBridge->isRunning()) {
-    this->pRosBridge->reset();
+  } else if (this->_pRosBridge->isRunning()) {
+    this->_pRosBridge->reset();
     this->topicServiceSetupDone = false;
   }
 
@@ -359,10 +444,10 @@ void NemoInterface::Impl::loop() {
     if (this->nextTimeout != TimePoint::max() &&
         this->nextTimeout < std::chrono::high_resolution_clock::now()) {
       lk.unlock();
-      if (this->pRosBridge->isRunning() && this->pRosBridge->connected()) {
-        this->setStatus(STATUS::WEBSOCKET_DETECTED);
+      if (this->_pRosBridge->isRunning() && this->_pRosBridge->connected()) {
+        this->_setState(STATUS::WEBSOCKET_DETECTED);
       } else {
-        this->setStatus(STATUS::TIMEOUT);
+        this->_setState(STATUS::TIMEOUT);
       }
     }
   }
@@ -382,7 +467,7 @@ void NemoInterface::Impl::publishTilesENU() {
       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");
+    this->_pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
   } else {
     qCWarning(NemoInterfaceLog)
         << "Impl::publishTilesENU: could not create json document.";
@@ -395,17 +480,17 @@ void NemoInterface::Impl::publishENUOrigin() {
       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");
+    this->_pRosBridge->publish(std::move(jOrigin), "/snake/origin");
   } else {
     qCWarning(NemoInterfaceLog)
         << "Impl::publishENUOrigin: could not create json document.";
   }
 }
 
-bool NemoInterface::Impl::setStatus(NemoInterface::STATUS s) {
+bool NemoInterface::Impl::_setState(STATE s) {
   if (s != this->status_) {
     this->status_ = s;
-    emit this->parent->statusChanged();
+    emit this->_parent->statusChanged();
     return true;
   } else {
     return false;
@@ -460,14 +545,18 @@ LogicalArray NemoInterface::containsTiles(const IDArray &idArray) {
   return this->pImpl->containsTiles(idArray);
 }
 
-TileArray NemoInterface::extractTiles(const IDArray &idArray) {
-  return this->pImpl->extractTiles(idArray);
-}
-
 std::size_t NemoInterface::size() { return this->pImpl->size(); }
 
 bool NemoInterface::empty() { return this->pImpl->empty(); }
 
+ProgressArray NemoInterface::getProgress() {
+  return this->pImpl->getProgress();
+}
+
+ProgressArray NemoInterface::getProgress(const IDArray &idArray) {
+  return this->pImpl->getProgress(idArray);
+}
+
 void NemoInterface::publishTileData() { this->pImpl->publishTileData(); }
 
 void NemoInterface::requestProgress() {

src/MeasurementComplexItem/NemoInterface.h

@@ -9,6 +9,7 @@
 #include "LogicalArray.h"
 #include "TileArray.h"
 #include "TilePtrArray.h"
+#include "geometry/ProgressArray.h"
 
 // Singelton class used to interface measurement devices implementing the nemo
 // interface.
@@ -26,11 +27,11 @@ public:
   static NemoInterface *instance();
 
   enum class STATUS {
-    NOT_CONNECTED = 0,
-    HEARTBEAT_DETECTED = 1,
-    WEBSOCKET_DETECTED = 2,
-    TIMEOUT = -1,
-    INVALID_HEARTBEAT = -2
+    NOT_CONNECTED,
+    READY,
+    WEBSOCKET_DETECTED,
+    TIMEOUT,
+    INVALID_HEARTBEAT
   };
   Q_ENUM(STATUS)
 
@@ -39,9 +40,12 @@ public:
   Q_PROPERTY(QString editorQml READ editorQml CONSTANT)
   Q_PROPERTY(bool running READ running NOTIFY runningChanged)
 
+  QString editorQml();
+
   Q_INVOKABLE void start();
   Q_INVOKABLE void stop();
 
+  // Tile editing.
   void addTiles(const TilePtrArray &tileArray);
   void addTiles(const TileArray &tileArray);
   void removeTiles(const IDArray &idArray);
@@ -49,18 +53,22 @@ public:
   TileArray getTiles(const IDArray &idArray);
   TileArray getAllTiles();
   LogicalArray containsTiles(const IDArray &idArray);
-  TileArray extractTiles(const IDArray &idArray);
   std::size_t size();
   bool empty();
 
+  // Progress.
+  ProgressArray getProgress();
+  ProgressArray getProgress(const IDArray &idArray);
+
+  // Status.
   STATUS status() const;
   QString statusString() const;
-  QString editorQml();
   bool running();
 
 signals:
   void statusChanged();
-  void progressChanged();
+  void progressChanged(const ProgressArray &progressArray);
+  void tilesChanged();
   void runningChanged();
 
 private:

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()
+{
+
+}

src/MeasurementComplexItem/nemo_interface/CommandDispatcher.h

+#ifndef COMMANDDISPATCHER_H
+#define COMMANDDISPATCHER_H
+
+
+class CommandDispatcher
+{
+public:
+    CommandDispatcher();
+};
+
+#endif // COMMANDDISPATCHER_H