SnakeDataManager mod

qgroundcontrol.pro

@@ -441,7 +441,7 @@ HEADERS += \
     src/Wima/Snake/SnakeTileLocal.h \
     src/Wima/Snake/SnakeTiles.h \
     src/Wima/Snake/SnakeTilesLocal.h \
-    src/Wima/Snake/SnakeW.h \
+    src/Wima/Snake/SnakeDataManager.h \
     src/Wima/WaypointManager/AreaInterface.h \
     src/Wima/WaypointManager/DefaultManager.h \
     src/Wima/WaypointManager/GenericWaypointManager.h \

src/Snake/snake.cpp

@@ -1041,7 +1041,6 @@ bool flight_plan::route(const BoostPolygon &area,
         route_idx.push_back(manager.IndexToNode(index).value());
     }
 
-
     // Helper Lambda.
     auto idx2Vertex = [&vertices](const std::vector<size_t> &idxArray,
                               std::vector<BoostPoint> &path){
@@ -1054,17 +1053,31 @@ bool flight_plan::route(const BoostPolygon &area,
         size_t idx0 = route_idx[i];
         size_t idx1 = route_idx[i+1];
         const auto &info1 = transectInfoList[idx0];
-        const auto &info2 = transectInfoList[idx0];
-        if ( info.front ){
-            const auto &t = transects[info.index];
-            for ( auto it = t.begin(); it < t.end()-1; ++it){
-                route.push_back(*it);
+        const auto &info2 = transectInfoList[idx1];
+        if (info1.index == info2.index) { // same transect?
+            if ( !info1.front ) { // transect reversal needed?
+                BoostLineString reversedTransect;
+                const auto &t = transects[info1.index];
+                for ( auto it = t.end()-1; it >= t.begin(); --it){
+                    reversedTransect.push_back(*it);
+                }
+                transectsRouted.push_back(reversedTransect);
+                for (auto it = reversedTransect.begin(); it < reversedTransect.end()-1; ++it){
+                    route.push_back(*it);
+                }
+            } else {
+                const auto &t = transects[info1.index];
+                for ( auto it = t.begin(); it < t.end()-1; ++it){
+                    route.push_back(*it);
+                }
+                transectsRouted.push_back(t);
             }
-            transectsRouted.push_back(t);
         } else {
             std::vector<size_t> idxList;
             shortestPathFromGraph(connectionGraph, idx0, idx1, idxList);
-            idxList.pop_back();
+            if ( i != route_idx.size()-2){
+                idxList.pop_back();
+            }
             idx2Vertex(idxList, route);
         }
     }

src/Snake/snake_typedefs.h

@@ -22,8 +22,21 @@ typedef bg::model::linestring<BoostPoint>               BoostLineString;
 typedef std::vector<std::vector<int64_t>>               Int64Matrix;
 typedef bg::model::box<BoostPoint>                      BoostBox;
 
-typedef bu::quantity<bu::si::length, double> Length;
-typedef bu::quantity<bu::si::area, double> Area;
-typedef bu::quantity<bu::si::plane_angle, double> Angle;
+typedef bu::quantity<bu::si::length> Length;
+typedef bu::quantity<bu::si::area> Area;
+typedef bu::quantity<bu::si::plane_angle> Angle;
 
 }
+
+namespace boost{
+namespace geometry{
+namespace model {
+bool operator==(snake::BoostPoint p1, snake::BoostPoint p2){
+    return (p1.get<0>() == p2.get<0>()) && (p1.get<1>() == p2.get<1>());
+}
+bool operator!=(snake::BoostPoint p1, snake::BoostPoint p2){
+    return !(p1 == p2);
+}
+} // namespace model
+} // namespace geometry
+} // namespace boost

src/Wima/Snake/SnakeDataManager.cc

@@ -17,71 +17,74 @@
 #include "Wima/Snake/SnakeTiles.h"
 #include "Wima/Snake/SnakeTilesLocal.h"
 
-#include "comm/ros_bridge/include/ros_bridge.h"
+#include "ros_bridge/include/ros_bridge.h"
+#include "ros_bridge/rapidjson/include/rapidjson/document.h"
+#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
+#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.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/progress.h"
+#include "ros_bridge/include/messages/nemo_msgs/heartbeat.h"
+
+
+#define EVENT_TIMER_INTERVAL 500 // ms
+#define TIMEOUT 3000 // ms
+
 
 using QVariantList = QList<QVariant>;
 using ROSBridgePtr = std::unique_ptr<ros_bridge::ROSBridge>;
 
 using UniqueLock = std::unique_lock<shared_timed_mutex>;
 using SharedLock = std::shared_lock<shared_timed_mutex>;
+using JsonDocUPtr = ros_bridge::com_private::JsonDocUPtr;
 
 class SnakeImpl : public QObject{
     Q_OBJECT
 public:
-    SnakeImpl() :
-        lineDistance(1*si::meter)
-      , minTransectLength(1*si::meter)
-      , calcInProgress(false)
-      , topicServiceSetupDone(false)
-    {
-        // ROS Bridge.
-        WimaSettings* wimaSettings = qgcApp()->toolbox()->settingsManager()->wimaSettings();
-        auto connectionStringFact = wimaSettings->rosbridgeConnectionString();
-        auto setConnectionString = [connectionStringFact, this]{
-            auto connectionString = connectionStringFact->rawValue().toString();
-            if ( ros_bridge::isValidConnectionString(connectionString.toLocal8Bit().data()) ){
-                this->pRosBridge.reset(new ros_bridge::ROSBridge(connectionString.toLocal8Bit().data()));
-            } else {
-                qgcApp()->showMessage("ROS Bridge connection string invalid: " + connectionString);
-                this->pRosBridge.reset(new ros_bridge::ROSBridge("localhost:9090"));
-            }
-        };
-        setConnectionString();
-        connect(connectionStringFact, &SettingsFact::rawValueChanged, setConnectionString);
-    }
+    SnakeImpl(SnakeDataManager* p);
+
+    bool precondition() const;
+    void resetWaypointData();
+    bool doTopicServiceSetup();
 
     // Private data.
-    ROSBridgePtr                pRosBridge;
-    bool                        topicServiceSetupDone;
-    mutable std::shared_timed_mutex   mutex;
-
-    // Input
-    snake::Scenario scenario;
-    Length lineDistance;
-    Length minTransectLength;
+    ROSBridgePtr                        pRosBridge;
+    bool                                rosBridgeEnabeled;
+    bool                                topicServiceSetupDone;
+    QTimer                              eventTimer;
+    QTimer                              timeout;
+    mutable std::shared_timed_mutex     mutex;
+
+    // Scenario
+    snake::Scenario       scenario;
+    Length                lineDistance;
+    Length                minTransectLength;
     QList<QGeoCoordinate> mArea;
     QList<QGeoCoordinate> sArea;
     QList<QGeoCoordinate> corridor;
+    QGeoCoordinate        ENUOrigin;
+    SnakeTiles            tiles;
+    QVariantList          tileCenterPoints;
+    SnakeTilesLocal       tilesENU;
+    QVector<QPointF>      tileCenterPointsENU;
 
-
-    // Output
-    std::atomic_bool            calcInProgress;
-    QNemoProgress               qProgress;
-    std::vector<int>            progress;
-    QNemoHeartbeat              heartbeat;
-
+    // Waypoints
     QVector<QGeoCoordinate>     waypoints;
     QVector<QGeoCoordinate>     arrivalPath;
     QVector<QGeoCoordinate>     returnPath;
-    QGeoCoordinate              ENUorigin;
     QVector<QPointF>            waypointsENU;
     QVector<QPointF>            arrivalPathENU;
     QVector<QPointF>            returnPathENU;
-    SnakeTiles                  tiles;
-    QVariantList                tileCenterPoints;
-    SnakeTilesLocal             tilesENU;
-    QVector<QPointF>            tileCenterPointsENU;
     QString                     errorMessage;
+
+
+    // Other
+    std::atomic_bool            calcInProgress;
+    QNemoProgress               qProgress;
+    std::vector<int>            progress;
+    QNemoHeartbeat              heartbeat;
+    SnakeDataManager           *parent;
+
 };
 
 template<class AtomicType>
@@ -107,7 +110,7 @@ private:
 
 SnakeDataManager::SnakeDataManager(QObject *parent)
     : QThread(parent)
-    , pImpl(std::make_unique<SnakeImpl>())
+    , pImpl(std::make_unique<SnakeImpl>(this))
 
 {
 }
@@ -135,16 +138,16 @@ void SnakeDataManager::setCorridor(const QList<QGeoCoordinate> &corridor)
     this->pImpl->corridor = corridor;
 }
 
-QNemoProgress SnakeDataManager::progress()
+QNemoProgress SnakeDataManager::nemoProgress()
 {
     SharedLock lk(this->pImpl->mutex);
     return this->pImpl->qProgress;
 }
 
-QNemoHeartbeat SnakeDataManager::heartbeat()
+int SnakeDataManager::nemoStatus()
 {
     SharedLock lk(this->pImpl->mutex);
-    return this->pImpl->heartbeat;
+    return this->pImpl->heartbeat.status();
 }
 
 bool SnakeDataManager::calcInProgress()
@@ -200,7 +203,17 @@ void SnakeDataManager::setTileWidth(Length tileWidth)
     this->pImpl->scenario.setTileWidth(tileWidth);
 }
 
-bool SnakeDataManager::precondition() const
+void SnakeDataManager::enableRosBridge()
+{
+    this->pImpl->rosBridgeEnabeled = true;
+}
+
+void SnakeDataManager::disableRosBride()
+{
+    this->pImpl->rosBridgeEnabeled = false;
+}
+
+bool SnakeImpl::precondition() const
 {
     return true;
 }
@@ -208,20 +221,20 @@ bool SnakeDataManager::precondition() const
 //!
 //! \brief Resets waypoint data.
 //! \pre this->_pImpl->mutex must be locked.
-void SnakeDataManager::resetWaypointData()
+void SnakeImpl::resetWaypointData()
 {
-    this->pImpl->waypoints.clear();
-    this->pImpl->arrivalPath.clear();
-    this->pImpl->returnPath.clear();
-    this->pImpl->ENUorigin = QGeoCoordinate(0.0,0.0,0.0);
-    this->pImpl->waypointsENU.clear();
-    this->pImpl->arrivalPathENU.clear();
-    this->pImpl->returnPathENU.clear();
-    this->pImpl->tiles.polygons().clear();
-    this->pImpl->tileCenterPoints.clear();
-    this->pImpl->tilesENU.polygons().clear();
-    this->pImpl->tileCenterPointsENU.clear();
-    this->pImpl->errorMessage.clear();
+    this->waypoints.clear();
+    this->arrivalPath.clear();
+    this->returnPath.clear();
+    this->ENUOrigin = QGeoCoordinate(0.0,0.0,0.0);
+    this->waypointsENU.clear();
+    this->arrivalPathENU.clear();
+    this->returnPathENU.clear();
+    this->tiles.polygons().clear();
+    this->tileCenterPoints.clear();
+    this->tilesENU.polygons().clear();
+    this->tileCenterPointsENU.clear();
+    this->errorMessage.clear();
 }
 
 void SnakeDataManager::run()
@@ -230,23 +243,23 @@ void SnakeDataManager::run()
     ToggleRAII<std::atomic_bool> tr(this->pImpl->calcInProgress);
 
     UniqueLock lk(this->pImpl->mutex);
-    resetWaypointData();
+    this->pImpl->resetWaypointData();
 
-    if ( !precondition() )
+    if ( !this->pImpl->precondition() )
         return;
 
     if ( this->pImpl->mArea.size() < 3) {
-        pImpl->errorMessage = "Measurement area invalid: size < 3.";
+        this->pImpl->errorMessage = "Measurement area invalid: size < 3.";
         return;
     }
     if ( this->pImpl->sArea.size() < 3) {
-        pImpl->errorMessage = "Service area invalid: size < 3.";
+        this->pImpl->errorMessage = "Service area invalid: size < 3.";
         return;
     }
 
 
-    this->pImpl->ENUorigin = this->pImpl->mArea.front();
-    auto &origin = this->pImpl->ENUorigin;
+    this->pImpl->ENUOrigin = this->pImpl->mArea.front();
+    auto &origin = this->pImpl->ENUOrigin;
     // Update measurement area.
     auto &mAreaEnu = this->pImpl->scenario.measurementArea();
     auto &mArea = this->pImpl->mArea;
@@ -278,17 +291,19 @@ void SnakeDataManager::run()
     }
 
     if ( !this->pImpl->scenario.update() ){
-        pImpl->errorMessage = this->pImpl->scenario.errorString.c_str();
+        this->pImpl->errorMessage = this->pImpl->scenario.errorString.c_str();
         return;
     }
 
     // Asynchronously update flightplan.
     qWarning() << "route calculation missing";
     std::string errorString;
-    auto future = std::async([this, &errorString]{
-        auto alpha = -this->pImpl->scenario.mAreaBoundingBox().angle*degree::degree;
+    auto future = std::async([this, &errorString, &origin]{
+        snake::Angle alpha(-this->pImpl->scenario.mAreaBoundingBox().angle*degree::degree);
         snake::flight_plan::Transects transects;
-        transects.push_back(bg::model::linestring{this->pImpl->scenario.homePositon()});
+        transects.push_back(bg::model::linestring<snake::BoostPoint>{
+                                this->pImpl->scenario.homePositon()
+                            });
         bool value = snake::flight_plan::transectsFromScenario(this->pImpl->lineDistance,
                                                              this->pImpl->minTransectLength,
                                                              alpha,
@@ -308,29 +323,44 @@ void SnakeDataManager::run()
         if ( !value )
             return value;
 
-        // Store waypoints.
-        for (auto boostVertex : _pFlightplan->waypoints()){
+        // Store arrival path.
+        const auto &firstWaypoint = transectsRouted.front().front();
+        long startIdx = 0;
+        for (long i = 0; i < route.size(); ++i){
+            const auto &boostVertex = route[i];
+            if ( boostVertex == firstWaypoint){
+                startIdx = i;
+                break;
+            }
             QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
             QGeoCoordinate geoVertex;
             snake::fromENU(origin, boostVertex, geoVertex);
-            pImpl->waypointsENU.push_back(enuVertex);
-            pImpl->waypoints.push_back(geoVertex);
+            this->pImpl->arrivalPathENU.push_back(enuVertex);
+            this->pImpl->arrivalPath.push_back(geoVertex);
         }
-        // Store arrival path.
-        for (auto boostVertex : _pFlightplan->arrivalPath()){
+        // Store return path.
+        long endIdx = 0;
+        const auto &lastWaypoint = transectsRouted.back().back();
+        for (long i = route.size()-1; i >= 0; --i){
+            const auto &boostVertex = route[i];
+            if ( boostVertex == lastWaypoint){
+                endIdx = i;
+                break;
+            }
             QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
             QGeoCoordinate geoVertex;
             snake::fromENU(origin, boostVertex, geoVertex);
-            pImpl->arrivalPathENU.push_back(enuVertex);
-            pImpl->arrivalPath.push_back(geoVertex);
+            this->pImpl->returnPathENU.push_back(enuVertex);
+            this->pImpl->returnPath.push_back(geoVertex);
         }
-        // Store return path.
-        for (auto boostVertex : _pFlightplan->returnPath()){
+        // Store waypoints.
+        for (long i = startIdx; i <= endIdx; ++i){
+            const auto &boostVertex = route[i];
             QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
             QGeoCoordinate geoVertex;
             snake::fromENU(origin, boostVertex, geoVertex);
-            pImpl->returnPathENU.push_back(enuVertex);
-            pImpl->returnPath.push_back(geoVertex);
+            this->pImpl->waypointsENU.push_back(enuVertex);
+            this->pImpl->waypoints.push_back(geoVertex);
         }
 
         return true;
@@ -359,10 +389,10 @@ void SnakeDataManager::run()
             QGeoCoordinate geoVertex;
             snake::fromENU(origin, boostPoint, geoVertex);
             geoTile.setCenter(geoVertex);
-            pImpl->tiles.polygons().push_back(geoTile);
-            pImpl->tileCenterPoints.push_back(QVariant::fromValue(geoVertex));
-            pImpl->tilesENU.polygons().push_back(enuTile);
-            pImpl->tileCenterPointsENU.push_back(enuVertex);
+            this->pImpl->tiles.polygons().push_back(geoTile);
+            this->pImpl->tileCenterPoints.push_back(QVariant::fromValue(geoVertex));
+            this->pImpl->tilesENU.polygons().push_back(enuTile);
+            this->pImpl->tileCenterPointsENU.push_back(enuVertex);
          }
     }
 
@@ -370,13 +400,187 @@ void SnakeDataManager::run()
     // Trying to generate flight plan.
     if ( !future.get() ){
         // error
-        for (auto c : _pFlightplan->errorString){
-            pImpl->errorMessage.push_back(QChar(c));
-        }
+        this->pImpl->errorMessage = errorString.c_str();
     } else {
         // Success!!!
 
     }
 }
 
+bool SnakeImpl::doTopicServiceSetup()
+{
+    using namespace ros_bridge::messages;
+    UniqueLock lk(this->mutex);
+
+    if ( this->tilesENU.polygons().size() == 0)
+        return false;
+
+    // Publish snake origin.
+    this->pRosBridge->advertiseTopic("/snake/origin",
+                                geographic_msgs::geo_point::messageType().c_str());
+    JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
+    bool ret = geographic_msgs::geo_point::toJson(
+                this->ENUOrigin, *jOrigin, jOrigin->GetAllocator());
+    Q_ASSERT(ret);
+    (void)ret;
+    this->pRosBridge->publish(std::move(jOrigin), "/snake/origin");
+
+
+    // Publish snake tiles.
+    this->pRosBridge->advertiseTopic("/snake/tiles",
+                                jsk_recognition_msgs::polygon_array::messageType().c_str());
+    JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
+    ret = jsk_recognition_msgs::polygon_array::toJson(
+                this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator());
+    Q_ASSERT(ret);
+    (void)ret;
+    this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
+
+
+    // Subscribe nemo progress.
+    this->pRosBridge->subscribe("/nemo/progress",
+                           /* callback */ [this](JsonDocUPtr pDoc){
+        UniqueLock lk(this->mutex);
+        int requiredSize = this->tilesENU.polygons().size();
+        auto& progressMsg = this->qProgress;
+        if ( !nemo_msgs::progress::fromJson(*pDoc, progressMsg)
+             || progressMsg.progress().size() != requiredSize ) { // Some error occured.
+            // Set progress to default.
+            progressMsg.progress().fill(0, requiredSize);
+
+            // Publish snake origin.
+            JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
+            bool ret = geographic_msgs::geo_point::toJson(
+                         this->ENUOrigin, *jOrigin, jOrigin->GetAllocator());
+            Q_ASSERT(ret);
+            (void)ret;
+            this->pRosBridge->publish(std::move(jOrigin), "/snake/origin");
+
+            // Publish snake tiles.
+            JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
+            ret = jsk_recognition_msgs::polygon_array::toJson(
+                         this->tilesENU, *jSnakeTiles, jSnakeTiles->GetAllocator());
+            Q_ASSERT(ret);
+            (void)ret;
+            this->pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
+        }
+        this->progress.clear();
+        for (const auto&p : progressMsg.progress()){
+            this->progress.push_back(p);
+        }
+        lk.unlock();
+
+        emit this->parent->nemoProgressChanged();
+    });
+
+
+    // Subscribe /nemo/heartbeat.
+    this->pRosBridge->subscribe("/nemo/heartbeat",
+                           /* callback */ [this](JsonDocUPtr pDoc){
+        UniqueLock lk(this->mutex);
+        if ( this->timeout.isActive() ) {
+            this->timeout.stop();
+        }
+        auto &heartbeatMsg = this->heartbeat;
+        if ( !nemo_msgs::heartbeat::fromJson(*pDoc, heartbeatMsg) ) {
+            heartbeatMsg.setStatus(integral(NemoStatus::InvalidHeartbeat));
+        }
+        this->timeout.singleShot(TIMEOUT, [this]{
+            UniqueLock lk(this->mutex);
+            this->heartbeat.setStatus(integral(NemoStatus::Timeout));
+            emit this->parent->nemoStatusChanged(integral(NemoStatus::Timeout));
+        });
+        emit this->parent->nemoStatusChanged(heartbeatMsg.status());
+    });
+
+
+    // Advertise /snake/get_origin.
+    this->pRosBridge->advertiseService("/snake/get_origin", "snake_msgs/GetOrigin",
+                                  [this](JsonDocUPtr) -> JsonDocUPtr
+    {
+        using namespace ros_bridge::messages;
+        SharedLock lk(this->mutex);
+
+        JsonDocUPtr pDoc(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
+        auto &origin = this->ENUOrigin;
+        rapidjson::Value jOrigin(rapidjson::kObjectType);
+        bool ret = geographic_msgs::geo_point::toJson(
+                    origin, jOrigin, pDoc->GetAllocator());
+        lk.unlock();
+        Q_ASSERT(ret);
+        (void)ret;
+        pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator());
+        return pDoc;
+    });
+
+
+    // Advertise /snake/get_tiles.
+    this->pRosBridge->advertiseService("/snake/get_tiles", "snake_msgs/GetTiles",
+                                  [this](JsonDocUPtr) -> JsonDocUPtr
+    {
+        SharedLock lk(this->mutex);
+
+        JsonDocUPtr pDoc(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
+        rapidjson::Value jSnakeTiles(rapidjson::kObjectType);
+        bool ret = jsk_recognition_msgs::polygon_array::toJson(
+                    this->tilesENU, jSnakeTiles, pDoc->GetAllocator());
+        Q_ASSERT(ret);
+        (void)ret;
+        pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator());
+        return pDoc;
+    });
+
+    return true;
+}
+
 
+
+SnakeImpl::SnakeImpl(SnakeDataManager *p) :
+    rosBridgeEnabeled(false)
+  , topicServiceSetupDone(false)
+  , lineDistance(1*si::meter)
+  , minTransectLength(1*si::meter)
+  , calcInProgress(false)
+  , parent(p)
+{
+    // ROS Bridge.
+    WimaSettings* wimaSettings = qgcApp()->toolbox()->settingsManager()->wimaSettings();
+    auto connectionStringFact = wimaSettings->rosbridgeConnectionString();
+    auto setConnectionString = [connectionStringFact, this]{
+        auto connectionString = connectionStringFact->rawValue().toString();
+        if ( ros_bridge::isValidConnectionString(connectionString.toLocal8Bit().data()) ){
+            this->pRosBridge.reset(new ros_bridge::ROSBridge(connectionString.toLocal8Bit().data()));
+        } else {
+            QString defaultString("localhost:9090");
+            qgcApp()->showMessage("ROS Bridge connection string invalid: " + connectionString);
+            qgcApp()->showMessage("Resetting connection string to: " + defaultString);
+            connectionStringFact->setRawValue(QVariant(defaultString)); // calls this function recursively
+        }
+    };
+    connect(connectionStringFact, &SettingsFact::rawValueChanged, setConnectionString);
+    setConnectionString();
+
+    // Periodic.
+    connect(&this->eventTimer, &QTimer::timeout, [this]{
+        if ( this->rosBridgeEnabeled ) {
+            if ( !this->pRosBridge->isRunning()) {
+                this->pRosBridge->start();
+            } else if ( this->pRosBridge->isRunning()
+                        && this->pRosBridge->connected()
+                        && !this->topicServiceSetupDone) {
+                if ( this->doTopicServiceSetup() )
+                    this->topicServiceSetupDone = true;
+            } else if ( this->pRosBridge->isRunning()
+                        && !this->pRosBridge->connected()
+                        && this->topicServiceSetupDone){
+                this->pRosBridge->reset();
+                this->pRosBridge->start();
+                this->topicServiceSetupDone = false;
+            }
+        } else  if ( this->pRosBridge->isRunning() ) {
+            this->pRosBridge->reset();
+            this->topicServiceSetupDone = false;
+        }
+    });
+    this->eventTimer.start(EVENT_TIMER_INTERVAL);
+}

src/Wima/Snake/SnakeDataManager.h

@@ -8,7 +8,7 @@
 #include "QNemoProgress.h"
 #include "QNemoHeartbeat.h"
 
-#include <boost/units/systems/si.hpp>>
+#include <boost/units/systems/si.hpp>
 
 
 using namespace boost::units;
@@ -17,6 +17,13 @@ using Area = quantity<si::area>;
 
 class SnakeImpl;
 
+enum class NemoStatus{
+    NotConnected = 0,
+    Connected = 1,
+    Timeout = -1,
+    InvalidHeartbeat = -2
+};
+
 class SnakeDataManager : public QThread{
     Q_OBJECT
 
@@ -31,8 +38,8 @@ public:
     void setServiceArea         (const QList<QGeoCoordinate>  &serviceArea);
     void setCorridor            (const QList<QGeoCoordinate>  &corridor);
 
-    QNemoProgress progress();
-    QNemoHeartbeat heartbeat();
+    QNemoProgress nemoProgress();
+    int nemoStatus();
     bool calcInProgress();
 
     Length lineDistance() const;
@@ -50,14 +57,17 @@ public:
     Length tileWidth() const;
     void setTileWidth(Length tileWidth);
 
+    void enableRosBridge();
+    void disableRosBride();
 
+signals:
+    void nemoProgressChanged();
+    void nemoStatusChanged(int status);
+    void calcInProgressChanged(bool inProgress);
 
 protected:
     void run() override;
-
 private:
-    bool precondition() const;
-    void resetWaypointData();
     SnakeImplPtr pImpl;
 };
 

src/Wima/WimaController.cc

@@ -2,14 +2,6 @@
 
 #include "utilities.h"
 
-#include "ros_bridge/rapidjson/include/rapidjson/document.h"
-#include "ros_bridge/rapidjson/include/rapidjson/writer.h"
-#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.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/progress.h"
-#include "ros_bridge/include/messages/nemo_msgs/heartbeat.h"
-
 #include "Snake/QNemoProgress.h"
 #include "Snake/QNemoHeartbeat.h"
 
@@ -45,10 +37,10 @@ const char* WimaController::snakeMinTransectLengthName  = "SnakeMinTransectLengt
 WimaController::StatusMap WimaController::_nemoStatusMap{
     std::make_pair<int, QString>(0, "No Heartbeat"),
     std::make_pair<int, QString>(1, "Connected"),
-    std::make_pair<int, QString>(-1, "Timeout")};
+    std::make_pair<int, QString>(-1, "Timeout"),
+    std::make_pair<int, QString>(-2, "Error")};
 
 using namespace snake;
-using namespace snake_geometry;
 
 WimaController::WimaController(QObject *parent)
     : QObject                   (parent)
@@ -125,11 +117,19 @@ WimaController::WimaController(QObject *parent)
     // Snake Worker Thread.
     connect(&_snakeDataManager, &SnakeDataManager::finished, this, &WimaController::_snakeStoreWorkerResults);
     connect(this, &WimaController::nemoProgressChanged, this, &WimaController::_initStartSnakeWorker);
-    connect(this, &QObject::destroyed, &this->_snakeDataManager, &SnakeWorker::quit);
+    connect(this, &QObject::destroyed, &this->_snakeDataManager, &SnakeDataManager::quit);
 
     // Snake.
-    connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_initStartSnakeWorker);
-    _initStartSnakeWorker();
+    auto configRosBridge = [this]{
+        if ( this->_enableSnake.rawValue().toBool() ){
+            this->_snakeDataManager.enableRosBridge();
+        } else {
+            this->_snakeDataManager.disableRosBride();
+        }
+    };
+    connect(&_enableSnake, &Fact::rawValueChanged, configRosBridge);
+    configRosBridge();
+
     connect(&_enableSnake, &Fact::rawValueChanged, this, &WimaController::_switchSnakeManager);
     _switchSnakeManager(_enableSnake.rawValue());
 }
@@ -202,8 +202,9 @@ Fact *WimaController::altitude() {
 
 QmlObjectListModel *WimaController::snakeTiles()
 {
+    static QmlObjectListModel list;
     qWarning() << "using snake tile dummy";
-    return QmlObjectListModel();
+    return &list;
 }
 
 QVariantList WimaController::snakeTileCenterPoints()
@@ -212,11 +213,10 @@ QVariantList WimaController::snakeTileCenterPoints()
     return QVariantList();
 }
 
-QVector<int> WimaController::nemoProgress() {
-    if ( _nemoProgress.progress().size() == _snakeTileCenterPoints.size() )
-        return _nemoProgress.progress();
-    else
-        return QVector<int>(_snakeTileCenterPoints.size(), 0);
+QVector<int> WimaController::nemoProgress()
+{
+    qWarning() << "using nemoProgress dummy";
+    return QVector<int>();
 }
 
 double WimaController::phaseDistance() const
@@ -231,17 +231,19 @@ double WimaController::phaseDuration() const
 
 int WimaController::nemoStatus() const
 {
-    return _nemoHeartbeat.status();
+    qWarning() << "using nemoStatus dummy";
+    return 0;
 }
 
 QString WimaController::nemoStatusString() const
 {
-    return _nemoStatusMap.at(_nemoHeartbeat.status());
+    return _nemoStatusMap.at(nemoStatus());
 }
 
 bool WimaController::snakeCalcInProgress() const
 {
-    return _snakeCalcInProgress;
+    qWarning() << "using snakeCalcInProgress dummy";
+    return false;
 }
 
 void WimaController::setMasterController(PlanMasterController *masterC)
@@ -762,28 +764,6 @@ void WimaController::_eventTimerHandler()
         emit WimaController::nemoStatusChanged();
         emit WimaController::nemoStatusStringChanged();
     }
-
-    if ( _snakeTicker.ready() ) {
-        if ( _enableSnake.rawValue().toBool() ) {
-            if ( !_pRosBridge->isRunning()) {
-                _pRosBridge->start();
-            } else if ( _pRosBridge->isRunning()
-                        && _pRosBridge->connected()
-                        && !_topicServiceSetupDone) {
-                if ( _doTopicServiceSetup() )
-                    _topicServiceSetupDone = true;
-            } else if ( _pRosBridge->isRunning()
-                        && !_pRosBridge->connected()
-                        && _topicServiceSetupDone){
-                _pRosBridge->reset();
-                _pRosBridge->start();
-                _topicServiceSetupDone = false;
-            }
-        } else  if ( _pRosBridge->isRunning() ) {
-                _pRosBridge->reset();
-                _topicServiceSetupDone = false;
-        }
-    }
 }
 
 void WimaController::_smartRTLCleanUp(bool flying)
@@ -965,114 +945,4 @@ void WimaController::_switchSnakeManager(QVariant variant)
     }
 }
 
-bool WimaController::_doTopicServiceSetup()
-{
-    using namespace ros_bridge::messages;
-
-    if ( _snakeTilesLocal.polygons().size() == 0)
-        return false;
-
-    // Publish snake origin.
-    _pRosBridge->advertiseTopic("/snake/origin",
-                                geographic_msgs::geo_point::messageType().c_str());
-    JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-    bool ret = geographic_msgs::geo_point::toJson(
-                _snakeOrigin, *jOrigin.get(), jOrigin->GetAllocator());
-    Q_ASSERT(ret);
-    (void)ret;
-    _pRosBridge->publish(std::move(jOrigin), "/snake/origin");
-
-
-    // Publish snake tiles.
-    _pRosBridge->advertiseTopic("/snake/tiles",
-                                jsk_recognition_msgs::polygon_array::messageType().c_str());
-    JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-    ret = jsk_recognition_msgs::polygon_array::toJson(
-                _snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator());
-    Q_ASSERT(ret);
-    (void)ret;
-    _pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
-
-
-    // Subscribe nemo progress.
-    _pRosBridge->subscribe("/nemo/progress",
-                           /* callback */ [this](JsonDocUPtr pDoc){
-        int requiredSize = this->_snakeTilesLocal.polygons().size();
-        auto& progress_msg = this->_nemoProgress;
-        if ( !nemo_msgs::progress::fromJson(*pDoc, progress_msg)
-             || progress_msg.progress().size() != requiredSize ) { // Some error occured.
-            // Set progress to default.
-            progress_msg.progress().fill(0, requiredSize);            
-
-            // Publish snake origin.
-            JsonDocUPtr jOrigin(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-            bool ret = geographic_msgs::geo_point::toJson(
-                         this->_snakeOrigin, *jOrigin, jOrigin->GetAllocator());
-            Q_ASSERT(ret);
-            (void)ret;
-            this->_pRosBridge->publish(std::move(jOrigin), "/snake/origin");
-
-            // Publish snake tiles.
-            JsonDocUPtr jSnakeTiles(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-            ret = jsk_recognition_msgs::polygon_array::toJson(
-                         this->_snakeTilesLocal, *jSnakeTiles, jSnakeTiles->GetAllocator());
-            Q_ASSERT(ret);
-            (void)ret;
-            this->_pRosBridge->publish(std::move(jSnakeTiles), "/snake/tiles");
-        }
-
-        emit WimaController::nemoProgressChanged();
-    });
-
-
-    // Subscribe /nemo/heartbeat.
-    _pRosBridge->subscribe("/nemo/heartbeat",
-                           /* callback */ [this](JsonDocUPtr pDoc){
-        auto &heartbeat_msg = this->_nemoHeartbeat;
-        if ( !nemo_msgs::heartbeat::fromJson(*pDoc, heartbeat_msg) ) {
-            if ( heartbeat_msg.status() == this->_fallbackStatus )
-                return;
-            heartbeat_msg.setStatus(this->_fallbackStatus);
-        }
 
-        this->_nemoTimeoutTicker.reset();
-        this->_fallbackStatus = -1; /*Timeout*/
-        emit WimaController::nemoStatusChanged();
-        emit WimaController::nemoStatusStringChanged();
-    });
-
-
-    // Advertise /snake/get_origin.
-    _pRosBridge->advertiseService("/snake/get_origin", "snake_msgs/GetOrigin",
-                                  [this](JsonDocUPtr) -> JsonDocUPtr
-    {
-        using namespace ros_bridge::messages;
-
-        JsonDocUPtr pDoc(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-        ::GeoPoint3D origin = this->_snakeOrigin;
-        rapidjson::Value jOrigin(rapidjson::kObjectType);
-        bool ret = geographic_msgs::geo_point::toJson(
-                    origin, jOrigin, pDoc->GetAllocator());
-        Q_ASSERT(ret);
-        (void)ret;
-        pDoc->AddMember("origin", jOrigin, pDoc->GetAllocator());
-        return pDoc;
-    });
-
-
-    // Advertise /snake/get_tiles.
-    _pRosBridge->advertiseService("/snake/get_tiles", "snake_msgs/GetTiles",
-                                  [this](JsonDocUPtr) -> JsonDocUPtr
-    {
-        JsonDocUPtr pDoc(std::make_unique<rapidjson::Document>(rapidjson::kObjectType));
-        rapidjson::Value jSnakeTiles(rapidjson::kObjectType);
-        bool ret = jsk_recognition_msgs::polygon_array::toJson(
-                    this->_snakeTilesLocal, jSnakeTiles, pDoc->GetAllocator());
-        Q_ASSERT(ret);
-        (void)ret;
-        pDoc->AddMember("tiles", jSnakeTiles, pDoc->GetAllocator());
-        return pDoc;
-    });
-
-    return true;
-}