123

src/Wima/Snake/NemoInterface.cpp

@@ -63,7 +63,7 @@ private:
   mutable std::shared_timed_mutex tilesENUMutex;
   QGeoCoordinate ENUOrigin;
   mutable std::shared_timed_mutex ENUOriginMutex;
-  QNemoProgress progress;
+  QNemoProgress qProgress;
   mutable std::shared_timed_mutex progressMutex;
   QNemoHeartbeat heartbeat;
   TimePoint nextTimeout;
@@ -105,7 +105,7 @@ NemoInterface::Impl::Impl(NemoInterface *p)
   setConnectionString();
 
   // Periodic.
-  connect(&this->loopTimer, &QTimer::timeout, this, &NemoInterface::Impl::loop);
+  connect(&this->loopTimer, &QTimer::timeout, [this] { this->loop(); });
   this->loopTimer.start(EVENT_TIMER_INTERVAL);
 }
 
@@ -140,14 +140,13 @@ NemoInterface::NemoStatus NemoInterface::Impl::status() {
 
 QVector<int> NemoInterface::Impl::progress() {
   SharedLock lk(this->progressMutex);
-  return this->progress.progress();
+  return this->qProgress.progress();
 }
 
 bool NemoInterface::Impl::doTopicServiceSetup() {
   using namespace ros_bridge::messages;
 
   // Publish snake tiles.
-  UniqueLock lk(this->mutex);
   {
     SharedLock lk(this->tilesENUMutex);
 
@@ -179,7 +178,7 @@ bool NemoInterface::Impl::doTopicServiceSetup() {
         UniqueLock lk3(this->ENUOriginMutex, std::adopt_lock);
 
         int requiredSize = this->tilesENU.polygons().size();
-        auto &progressMsg = this->progress;
+        auto &progressMsg = this->qProgress;
         if (!nemo_msgs::progress::fromJson(*pDoc, progressMsg) ||
             progressMsg.progress().size() !=
                 requiredSize) { // Some error occured.
@@ -207,7 +206,7 @@ bool NemoInterface::Impl::doTopicServiceSetup() {
         lk2.unlock();
         lk3.unlock();
 
-        emit this->parent->nemoProgressChanged();
+        emit this->parent->progressChanged();
       });
 
   // Subscribe /nemo/heartbeat.
@@ -225,7 +224,7 @@ bool NemoInterface::Impl::doTopicServiceSetup() {
               std::chrono::high_resolution_clock::now() + timeoutInterval;
         }
 
-        emit this->parent->nemoStatusChanged(heartbeatMsg.status());
+        emit this->parent->statusChanged();
       });
 
   // Advertise /snake/get_origin.
@@ -233,7 +232,7 @@ bool NemoInterface::Impl::doTopicServiceSetup() {
       "/snake/get_origin", "snake_msgs/GetOrigin",
       [this](JsonDocUPtr) -> JsonDocUPtr {
         using namespace ros_bridge::messages;
-        SharedLock lk(this->mutex);
+        SharedLock lk(this->ENUOriginMutex);
 
         JsonDocUPtr pDoc(
             std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
@@ -252,7 +251,7 @@ bool NemoInterface::Impl::doTopicServiceSetup() {
   this->pRosBridge->advertiseService(
       "/snake/get_tiles", "snake_msgs/GetTiles",
       [this](JsonDocUPtr) -> JsonDocUPtr {
-        SharedLock lk(this->mutex);
+        SharedLock lk(this->tilesENUMutex);
 
         JsonDocUPtr pDoc(
             std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
@@ -294,16 +293,17 @@ void NemoInterface::Impl::loop() {
       this->nextTimeout != TimePoint::max()) {
     if (this->nextTimeout < std::chrono::high_resolution_clock::now()) {
       UniqueLock lk(this->heartbeatMutex);
-      this->heartbeat.setStatus(NemoStatus::Timeout);
+      this->heartbeat.setStatus(integral(NemoStatus::Timeout));
       emit this->parent->statusChanged();
     }
   }
 }
 
 void NemoInterface::Impl::publishTilesENU() {
+  using namespace ros_bridge::messages;
   JsonDocUPtr jSnakeTiles(
       std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-  ret = jsk_recognition_msgs::polygon_array::toJson(
+  bool ret = jsk_recognition_msgs::polygon_array::toJson(
       this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator());
   Q_ASSERT(ret);
   (void)ret;
@@ -311,6 +311,7 @@ void NemoInterface::Impl::publishTilesENU() {
 }
 
 void NemoInterface::Impl::publishENUOrigin() {
+  using namespace ros_bridge::messages;
   JsonDocUPtr jOrigin(
       std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
   bool ret = geographic_msgs::geo_point::toJson(this->ENUOrigin, *jOrigin,
@@ -336,6 +337,4 @@ NemoInterface::NemoStatus NemoInterface::status() {
   return this->pImpl->status();
 }
 
-QVector<int> NemoInterface::progress() {
-  return this->pImpl->progress.progress();
-}
+QVector<int> NemoInterface::progress() { return this->pImpl->progress(); }

src/Wima/Snake/SnakeDataManager.h

@@ -5,9 +5,9 @@
 #include <QObject>
 #include <QThread>
 
-#include "QNemoHeartbeat.h"
 #include "QNemoProgress.h"
 #include "SnakeTiles.h"
+#include "SnakeTilesLocal.h"
 
 #include <boost/units/systems/si.hpp>
 
@@ -18,8 +18,8 @@ using Area = quantity<si::area>;
 class SnakeDataManager : public QThread {
   Q_OBJECT
 
-  class SnakeImpl;
-  using SnakeImplPtr = std::unique_ptr<SnakeImpl>;
+  class Impl;
+  using PImpl = std::unique_ptr<Impl>;
 
 public:
   SnakeDataManager(QObject *parent = nullptr);
@@ -29,9 +29,11 @@ public:
   void setServiceArea(const QList<QGeoCoordinate> &serviceArea);
   void setCorridor(const QList<QGeoCoordinate> &corridor);
 
-  const QmlObjectListModel *tiles() const;
+  SnakeTiles tiles() const;
+  SnakeTilesLocal tilesENU() const;
+  QGeoCoordinate ENUOrigin() const;
   QVariantList tileCenterPoints() const;
-  QNemoProgress nemoProgress() const;
+  QNemoProgress progress() const;
   bool calcInProgress() const;
   QString errorMessage() const;
   bool success() const;
@@ -62,5 +64,5 @@ protected:
   void run() override;
 
 private:
-  SnakeImplPtr pImpl;
+  PImpl pImpl;
 };

src/Wima/WimaController.cc

@@ -194,7 +194,7 @@ QVariantList WimaController::snakeTileCenterPoints() {
 }
 
 QVector<int> WimaController::nemoProgress() {
-  return _currentDM->nemoProgress().progress();
+  return _currentDM->progress().progress();
 }
 
 double WimaController::phaseDistance() const {