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.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/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/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