temp

qgroundcontrol.pro

@@ -453,6 +453,7 @@ HEADERS += \
     src/MeasurementComplexItem/geometry/TileDiff.h \
     src/MeasurementComplexItem/geometry/geometry.h \
     src/MeasurementComplexItem/HashFunctions.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 \
@@ -517,7 +518,7 @@ HEADERS += \
     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/progress.h \
+    src/comm/ros_bridge/include/messages/nemo_msgs/tile.h \
     src/comm/ros_bridge/include/messages/std_msgs/header.h \
     src/comm/ros_bridge/include/server.h \
     src/comm/ros_bridge/include/messages/std_msgs/time.h \
@@ -564,7 +565,8 @@ SOURCES += \
     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/progress.cpp \
+    src/comm/ros_bridge/include/messages/nemo_msgs/labeled_progress.cpp \
+    src/comm/ros_bridge/include/messages/nemo_msgs/tile.cpp \
     src/comm/ros_bridge/include/messages/std_msgs/header.cpp \
     src/comm/ros_bridge/include/messages/std_msgs/time.cpp \
     src/comm/ros_bridge/include/server.cpp \

src/MeasurementComplexItem/NemoInterface.cpp

@@ -79,15 +79,17 @@ public:
   NemoInterface::STATUS status();
   bool running();
 
+private slots:
+  void _addTilesLocal(const TileArray &tileArray);
+  void _removeTilesLocal(const IDArray &idArray);
+  void _clearTilesLocal();
+  void _updateProgress(ProgressArray progressArray);
+
 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();
 
@@ -158,6 +160,8 @@ void NemoInterface::Impl::stop() {
   emit this->_parent->runningChanged();
 }
 
+void NemoInterface::Impl::addTiles(const TilePtrArray &tileArray) {}
+
 TileArray NemoInterface::Impl::getTiles(const IDArray &idArray) {
   TileArray tileArray;
 
@@ -292,24 +296,76 @@ QVector<int> NemoInterface::Impl::progress() {
 
 bool NemoInterface::Impl::running() { return _running(this->_state); }
 
-void NemoInterface::Impl::doTopicServiceSetup() {
-  using namespace ros_bridge::messages;
+void NemoInterface::Impl::_addTilesLocal(const TileArray &tileArray) {
+  bool anyChanges = false;
+
+  for (const auto &tile : tileArray) {
+    const auto id = tile.id();
+    const auto it = _tileMap.find(id);
+    if (Q_LIKELY(it == _tileMap.end())) {
+      auto ret = _tileMap.insert(std::make_pair(id, tile));
+      anyChanges = true;
+      Q_ASSERT(ret.second == true);
+      Q_UNUSED(ret);
+    } else {
+      qCDebug(NemoInterfaceLog)
+          << "_addTilesLocal(): tile with id " << id << " already inserted.";
+    }
+  }
+
+  if (anyChanges) {
+    emit _parent->tilesChanged();
+  }
+}
+
+void NemoInterface::Impl::_removeTilesLocal(const IDArray &idArray) {
+  bool anyChanges = false;
+  for (const auto &id : idArray) {
+    const auto it = _tileMap.find(id);
+    if (Q_LIKELY(it != _tileMap.end())) {
+      _tileMap.erase(it);
+      anyChanges = true;
+    } else {
+      qCDebug(NemoInterfaceLog)
+          << "_removeTilesLocal(): tile with id " << id << " not found.";
+    }
+  }
+
+  if (anyChanges) {
+    emit _parent->tilesChanged();
+  }
+}
 
-  // snake tiles.
-  {
-    SharedLock lk(this->tilesENUMutex);
-    this->_pRosBridge->advertiseTopic(
-        "/snake/tiles",
-        jsk_recognition_msgs::polygon_array::messageType().c_str());
+void NemoInterface::Impl::_clearTilesLocal() {
+  if (!_tileMap.empty()) {
+    _tileMap.clear();
+    emit _parent->tilesChanged();
   }
+}
+
+void NemoInterface::Impl::_updateProgress(ProgressArray progressArray) {
+  for (auto itPair = progressArray.begin(); itPair != progressArray.end();) {
+
+    const auto &id = itPair->first;
+    auto it = _tileMap.find(id);
 
-  // snake origin.
-  {
-    SharedLock lk(this->ENUOriginMutex);
-    this->_pRosBridge->advertiseTopic(
-        "/snake/origin", geographic_msgs::geo_point::messageType().c_str());
+    if (Q_LIKELY(it != _tileMap.end())) {
+      const auto &progress = itPair->second;
+      it->second.setProgress(progress);
+      ++itPair;
+    } else {
+      qCDebug(NemoInterfaceLog)
+          << "_updateProgress(): tile with id " << id << " not found.";
+      itPair = progressArray.erase(itPair);
+    }
   }
 
+  emit _parent->progressChanged(progressArray);
+}
+
+void NemoInterface::Impl::doTopicServiceSetup() {
+  using namespace ros_bridge::messages;
+
   // Subscribe nemo progress.
   this->_pRosBridge->subscribe(
       "/nemo/progress",
@@ -340,7 +396,6 @@ void NemoInterface::Impl::doTopicServiceSetup() {
   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->_setState(STATUS::INVALID_HEARTBEAT);
@@ -355,61 +410,6 @@ void NemoInterface::Impl::doTopicServiceSetup() {
           this->nextTimeout =
               std::chrono::high_resolution_clock::now() + timeoutInterval;
         }
-
-        //        auto delta =
-        //        std::chrono::duration_cast<std::chrono::milliseconds>(
-        //            std::chrono::high_resolution_clock::now() - start);
-        //        std::cout << "/nemo/heartbeat callback time: " <<
-        //        delta.count() << " ms"
-        //                  << std::endl;
-      });
-
-  // Advertise /snake/get_origin.
-  this->_pRosBridge->advertiseService(
-      "/snake/get_origin", "snake_msgs/GetOrigin",
-      [this](JsonDocUPtr) -> JsonDocUPtr {
-        using namespace ros_bridge::messages;
-        SharedLock lk(this->ENUOriginMutex);
-
-        JsonDocUPtr pDoc(
-            std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-        auto &origin = this->ENUOrigin;
-        rapidjson::Value jOrigin(rapidjson::kObjectType);
-        lk.unlock();
-        if (geographic_msgs::geo_point::toJson(origin, jOrigin,
-                                               pDoc->GetAllocator())) {
-          lk.unlock();
-          pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator());
-        } else {
-          lk.unlock();
-          qCWarning(NemoInterfaceLog)
-              << "/snake/get_origin service: could not create json document.";
-        }
-
-        return pDoc;
-      });
-
-  // Advertise /snake/get_tiles.
-  this->_pRosBridge->advertiseService(
-      "/snake/get_tiles", "snake_msgs/GetTiles",
-      [this](JsonDocUPtr) -> JsonDocUPtr {
-        SharedLock lk(this->tilesENUMutex);
-
-        JsonDocUPtr pDoc(
-            std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-        rapidjson::Value jSnakeTiles(rapidjson::kObjectType);
-
-        if (jsk_recognition_msgs::polygon_array::toJson(
-                this->tilesENU, jSnakeTiles, pDoc->GetAllocator())) {
-          lk.unlock();
-          pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator());
-        } else {
-          lk.unlock();
-          qCWarning(NemoInterfaceLog)
-              << "/snake/get_tiles service: could not create json document.";
-        }
-
-        return pDoc;
       });
 }
 

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

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

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

@@ -12,67 +12,57 @@ namespace messages {
 namespace nemo_msgs {
 //! @brief Namespace containing methodes for geometry_msgs/Point32 message
 //! generation.
-namespace progress {
+namespace labeled_progress {
 
 std::string messageType();
 
-//! @brief C++ representation of nemo_msgs/Progress message
-template <class IntType = long,
-          template <class, class...> class ContainterType = std::vector>
-class GenericProgress {
-public:
-  GenericProgress() {}
-  GenericProgress(const ContainterType<IntType> &progress)
-      : _progress(progress) {}
-  GenericProgress(const GenericProgress &p) : _progress(p.progress()) {}
+namespace {
+const char *progressKey = "progress";
+const char *idKey = "id";
+} // namespace
 
-  virtual const ContainterType<IntType> &progress(void) const {
-    return _progress;
-  }
-  virtual ContainterType<IntType> &progress(void) { return _progress; }
+//! @brief C++ representation of nemo_msgs/labeled_progress message
+class LabeledProgress {
+public:
+  LabeledProgress() {}
+  LabeledProgress(double progress, long id) : _id(id), _progress(progress) {}
 
-  bool operator==(const GenericProgress &other) const {
-    return this->_progress == other._progress;
-  }
+  long id() const { return _id; }
+  void setId(long id) { _id = id; }
 
-  bool operator!=(const GenericProgress &other) const {
-    return !this->operator==(other);
-  }
+  double progress() const { return _progress; }
+  void setProgress(double progress) { _progress = progress; }
 
 protected:
-  ContainterType<IntType> _progress;
+  long _id;
+  double _progress;
 };
-typedef GenericProgress<> Progress;
 
-template <class ProgressType>
-bool toJson(const ProgressType &p, rapidjson::Value &value,
+template <class LabeledProgressType>
+bool toJson(const LabeledProgressType &lp, rapidjson::Value &value,
             rapidjson::Document::AllocatorType &allocator) {
-  rapidjson::Value jProgress(rapidjson::kArrayType);
-  for (unsigned long i = 0; i < std::uint64_t(p.progress().size()); ++i) {
-    jProgress.PushBack(rapidjson::Value().SetInt(std::int32_t(p.progress()[i])),
-                       allocator);
-  }
-  value.AddMember("progress", jProgress, allocator);
+  value.AddMember(idKey, lb.id(), allocator);
+  value.AddMember(progressKey, lb.progress(), allocator);
   return true;
 }
 
 template <class ProgressType>
 bool fromJson(const rapidjson::Value &value, ProgressType &p) {
-  if (!value.HasMember("progress") || !value["progress"].IsArray()) {
-    assert(false);
+  if (!value.HasMember(progressKey) || !value[progressKey].IsDouble()) {
     return false;
   }
 
-  const auto &jsonProgress = value["progress"];
-  unsigned long sz = jsonProgress.Size();
-  p.progress().clear();
-  p.progress().reserve(sz);
-  for (unsigned long i = 0; i < sz; ++i)
-    p.progress().push_back(std::int32_t(jsonProgress[i].GetInt()));
+  if (!value.HasMember(idKey) || !value[idKey].IsInt()) {
+    return false;
+  }
+
+  p.setId(value[idKey].GetInt());
+  p.setProgress(value[progressKey].GetDouble());
+
   return true;
 }
 
-} // namespace progress
+} // namespace labeled_progress
 } // namespace nemo_msgs
 } // namespace messages
 } // namespace ros_bridge

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

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

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

+#include "tile.h"
+
+tile::tile()
+{
+
+}

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

+#pragma once
+
+#include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
+#include "ros_bridge/rapidjson/include/rapidjson/document.h"
+
+#include <vector>
+
+namespace ros_bridge {
+//! @brief Namespace containing classes and methodes ros message generation.
+namespace messages {
+//! @brief Namespace containing classes and methodes for nemo_msgs
+//! generation.
+namespace nemo_msgs {
+//! @brief Namespace containing methodes for nemo_msgs/tile message
+//! generation.
+namespace labeled_progress {
+
+std::string messageType();
+
+namespace {
+const char *progressKey = "progress";
+const char *idKey = "id";
+const char *tileKey = "tile";
+} // namespace
+
+//! @brief C++ representation of nemo_msgs/tile message
+template <class GeoPoint, template <class, class> class Container = std::vector>
+class GenericTile {
+public:
+  typedef Container<GeoPoint> GeoContainer;
+  GenericTile() {}
+  GenericTile(const GeoContainer &tile, double progress, long id)
+      : _tile(tile), _id(id), _progress(progress) {}
+
+  long id() const { return _id; }
+  void setId(long id) { _id = id; }
+
+  double progress() const { return _progress; }
+  void setProgress(double progress) { _progress = progress; }
+
+  const GeoContainer &tile() const { return _tile; }
+  GeoContainer &tile() { return _tile; }
+  void setTile(const GeoContainer &tile) { _tile = tile; }
+
+protected:
+  long _id;
+  double _progress;
+  GeoContainer _tile;
+};
+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 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);
+    if (!geo_point::toJson(tile.tile()[i], geoPoint, allocator)) {
+      return false;
+    }
+    geoPoints.PushBack(geoPoint, allocator);
+  }
+  value.AddMember(tileKey, geoPoints, allocator);
+
+  return true;
+}
+
+template <class ProgressType>
+bool fromJson(const rapidjson::Value &value, ProgressType &p) {
+  if (!value.HasMember(progressKey) || !value[progressKey].IsDouble()) {
+    return false;
+  }
+
+  if (!value.HasMember(idKey) || !value[idKey].IsInt()) {
+    return false;
+  }
+
+  p.setId(value[idKey].GetInt());
+  p.setProgress(value[progressKey].GetDouble());
+
+  return true;
+}
+
+} // namespace labeled_progress
+} // namespace nemo_msgs
+} // namespace messages
+} // namespace ros_bridge